
#ifndef __COMM_CFG_PARAM_CPP__
#define __COMM_CFG_PARAM_CPP__

#include "comm/comm_cfg_param.h"
#include "comm/comm_base.h"
#include "comm/comm_file.h"

#include <string.h>
#include <math.h>
#include <stdio.h>
#include <iostream>
#include <list>
#include <vector>

using namespace std;

/**************************************************************************************
功能描述: 读取配置文件
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::Read(CFG_PARAM_STRU &stParam)
{
    string         strCfgParamFilePath;
    CFG_FILE_MNG_C objCfgFileMng;
    objCfgFileMng.GetCfgParamFilePath(strCfgParamFilePath);
    
    TiXmlDocument objXmlFile(strCfgParamFilePath.c_str());
    if (!objXmlFile.LoadFile()) {
        ST_LOG_ERR("Load File(%s) Fail.", strCfgParamFilePath.c_str());
        return -1;
    }

    TiXmlElement *pobjCfgParam = objXmlFile.RootElement();
    if (pobjCfgParam == NULL) {
        ST_LOG_ERR("Get Root Element Fail.");
        return -1;
    }

#define ReadDevCfg_(pfRead_, stDevCfg_)                         \
    do {                                                        \
        INT32 rslt = pfRead_(pobjCfgParam, stDevCfg_);          \
        if (rslt != 0) {                                        \
            ST_LOG_ERR(#pfRead_"() Fail.");                     \
            return -1;                                          \
        }                                                       \
    } while(0)

    ReadDevCfg_(ReadGpsCfg,                     stParam.stGps);
    ReadDevCfg_(ReadImuCfg,                     stParam.stImu);
    ReadDevCfg_(ReadLocLidarCfg,                stParam.stLocLidar);
    ReadDevCfg_(ReadHeadDepthCameraCfg,         stParam.stHeadDepthCamera);
    ReadDevCfg_(ReadBackDepthCameraCfg,         stParam.stBackDepthCamera);
    ReadDevCfg_(ReadLeftBackDepthCameraCfg,     stParam.stLeftBackDepthCamera);
    ReadDevCfg_(ReadRightBackDepthCameraCfg,    stParam.stRightBackDepthCamera);
    ReadDevCfg_(ReadVisionDetectCfg,            stParam.stVisionDetect);
    ReadDevCfg_(ReadLidarCfg,                   stParam.stLidar);
    ReadDevCfg_(ReadMultiLidarFusionCfg,        stParam.stMultiLidarFusion);
    ReadDevCfg_(ReadLocalizationCfg,            stParam.stLoc);
    ReadDevCfg_(ReadRadarCfg,                   stParam.stRadar);
    ReadDevCfg_(ReadPcdCfg,                     stParam.stPcd);
    ReadDevCfg_(ReadVehicleChassisCfg,          stParam.stVehicleChassis);
    ReadDevCfg_(ReadPlanCfg,                    stParam.stPlan);

#undef ReadDevCfg_

    ST_LOG_INFO("Succ.");
    return 0;
}

/**************************************************************************************
功能描述: 写入配置文件
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::Save(const CFG_PARAM_STRU &stParam)
{
    return -1;
}

TiXmlElement *CFG_PARAM_FILE_C::GetFirstChildElement(TiXmlElement *pobjCfgParam, const char *element)
{
    if (pobjCfgParam == NULL) {
        ST_LOG_ERR("pobjCfgParam==NULL.");
        return NULL;
    }

    TiXmlElement *pTiXmlElement = pobjCfgParam->FirstChildElement(element);

    if(pTiXmlElement == NULL) {
        ST_LOG_ERR("Get <%s> Cfg Element Fail.", element);
        return NULL;
    }

    return pTiXmlElement;
}

TiXmlElement *CFG_PARAM_FILE_C::GetNextSiblingElement(TiXmlElement *pobjCfgParam, const char *element)
{
    if (pobjCfgParam == NULL) {
        ST_LOG_ERR("pobjCfgParam==NULL.");
        return NULL;
    }

    TiXmlElement *pTiXmlElement = pobjCfgParam->NextSiblingElement(element);

    if(pTiXmlElement == NULL) {
        ST_LOG_ERR("Get <%s> Cfg Element Fail.", element);
        return NULL;
    }

    return pTiXmlElement;
}

INT32 CFG_PARAM_FILE_C::GetBoolAttributeValue(TiXmlElement *pobjCfgParam, const char *element, const char *attribute, BOOL &value)
{
    if (pobjCfgParam==NULL) {
        ST_LOG_ERR("pobjCfgParam==NULL, Element <%s> Object Not Exits", element);
        return -1;
    }

    if(TIXML_SUCCESS != pobjCfgParam->QueryBoolAttribute(attribute, &value)) {
        ST_LOG_ERR("Get <%s> Cfg Attr <%s> Fail.", element, attribute);
        return -1;
    }
    return 0;
}

INT32 CFG_PARAM_FILE_C::GetIntAttributeValue(TiXmlElement *pobjCfgParam, const char *element, const char *attribute, INT32 &value)
{
    if (pobjCfgParam==NULL) {
        ST_LOG_ERR("pobjCfgParam==NULL, Element <%s> Object Not Exits", element);
        return -1;
    }

    if(TIXML_SUCCESS != pobjCfgParam->QueryIntAttribute(attribute, &value)) {
        ST_LOG_ERR("Get <%s> Cfg Attr <%s> Fail.", element, attribute);
        return -1;
    }
    return 0;
}

INT32 CFG_PARAM_FILE_C::GetUnsignedAttributeValue(TiXmlElement *pobjCfgParam, const char *element, const char *attribute, UINT32 &value)
{
    if (pobjCfgParam==NULL) {
        ST_LOG_ERR("pobjCfgParam==NULL, Element <%s> Object Not Exits", element);
        return -1;
    }

    if(TIXML_SUCCESS != pobjCfgParam->QueryUnsignedAttribute(attribute, &value)) {
        ST_LOG_ERR("Get <%s> Cfg Attr <%s> Fail.", element, attribute);
        return -1;
    }
    return 0;
}

INT32 CFG_PARAM_FILE_C::GetFloatAttributeValue(TiXmlElement *pobjCfgParam, const char *element, const char *attribute, float &value)
{
    if (pobjCfgParam==NULL) {
        ST_LOG_ERR("pobjCfgParam==NULL, Element <%s> Object Not Exits", element);
        return -1;
    }

    if(TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute(attribute, &value)) {
        ST_LOG_ERR("Get <%s> Cfg Attr <%s> Fail.", element, attribute);
        return -1;
    }
    return 0;
}

INT32 CFG_PARAM_FILE_C::ReadInstallParam(TiXmlElement *pobjCfgParam, const char *element, CFG_PARAM_INSTALL_STRU &stInstallPose)
{
    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Tx",    stInstallPose.tx);
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Ty",    stInstallPose.ty);
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Tz",    stInstallPose.tz);
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Roll",  stInstallPose.roll);
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Pitch", stInstallPose.pitch);
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Yaw",   stInstallPose.yaw);

//    ST_LOG_INFO("Tx:%f Ty:%f Tz:%f Roll:%f Pitch:%f Yaw:%f", stInstallPose.tx, stInstallPose.ty, stInstallPose.tz, stInstallPose.pitch, stInstallPose.roll, stInstallPose.yaw);
    return rslt;
}

INT32 CFG_PARAM_FILE_C::ReadInstallPosParam(TiXmlElement *pobjCfgParam, const char *element, CFG_PARAM_INSTALL_POS_STRU &stInstallPos)
{
    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Tx", stInstallPos.tx);
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Ty", stInstallPos.ty);
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Tz", stInstallPos.tz);

//    ST_LOG_INFO("Tx:%f Ty:%f Tz:%f", stInstallPos.tx, stInstallPos.ty, stInstallPos.tz);
    return rslt;
}

INT32 CFG_PARAM_FILE_C::ReadDetectObjectParam(TiXmlElement *pobjCfgParam, const char *element, CFG_PARAM_VISION_DETECT_OBJECT_STRU &stDetectObj)
{
    INT32 rslt = 0;
    rslt += GetBoolAttributeValue( pobjCfgParam, element, "isSupport", stDetectObj.isSupport);
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "minDist",   stDetectObj.minDist);
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "maxDist",   stDetectObj.maxDist);

//    ST_LOG_INFO("isSupport:%d minDist:%f maxDist:%f", stDetectObj.isSupport, stDetectObj.minDist, stDetectObj.maxDist);
    return rslt;
}

/**************************************************************************************
功能描述: 读取GPS配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadGpsCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_GPS_STRU &stGpsCfg)
{
    TiXmlElement *pobjGpsCfg = pobjCfgParam->FirstChildElement("Gps");
    if (pobjGpsCfg == NULL) {
        ST_LOG_ERR("Get <Gps> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjGpsCfg->QueryBoolAttribute("isSupport", &stGpsCfg.isSupport)) {
        ST_LOG_ERR("Get <Gps> Cfg Attr <isSupport> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjGpsCfg->QueryBoolAttribute("isDualAntenna", &stGpsCfg.isDualAntenna)) {
        ST_LOG_ERR("Get <Gps> Cfg Attr <isDualAntenna> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjGpsCfg->QueryStringAttribute("devName", &stGpsCfg.strDevName)) {
        ST_LOG_ERR("Get <Gps> Cfg Attr <devName> Fail.");
        return -1;
    }
    if (TIXML_SUCCESS != pobjGpsCfg->QueryIntAttribute("baudRate", &stGpsCfg.baudRate)) {
        ST_LOG_ERR("Get <Gps> Cfg Attr <baudRate> Fail.");
        return -1;
    }
    TiXmlElement *pobjInstallParamCfg = GetFirstChildElement(pobjGpsCfg, "InstallParam");
    if (pobjInstallParamCfg == NULL) 
        return -1;

    INT32 rslt = ReadInstallParam(pobjInstallParamCfg, "InstallParam", stGpsCfg.stInstallPose);
    if (rslt != 0) {
        ST_LOG_ERR("Read Gps Install Pose Param Fail.");    
        return -1;
    }

    ST_LOG_INFO("Cfg Param: <isSupport> = %d.", stGpsCfg.isSupport);
    return 0;
}

/**************************************************************************************
功能描述: 读取IMU配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadImuCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_IMU_STRU &stImuCfg)
{
    TiXmlElement *pobjImuCfg = pobjCfgParam->FirstChildElement("Imu");
    if (pobjImuCfg == NULL) {
        ST_LOG_ERR("Get <Imu> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjImuCfg->QueryBoolAttribute("isSupport", &stImuCfg.isSupport)) {
        ST_LOG_ERR("Get <Imu> Cfg Attr <isSupport> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjImuCfg->QueryStringAttribute("devName", &stImuCfg.strDevName)) {
        ST_LOG_ERR("Get <Imu> Cfg Attr <devName> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjImuCfg->QueryIntAttribute("baudRate", &stImuCfg.baudRate)) {
        ST_LOG_ERR("Get <Imu> Cfg Attr <baudRate> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjImuCfg->QueryBoolAttribute("isModifyGyroZ", &stImuCfg.isModifyGyroZ)) {
        ST_LOG_ERR("Get <Imu> Cfg Attr <isModifyGyroZ> Fail.");
        return -1;
    }

    TiXmlElement *pobjInstallParamCfg = GetFirstChildElement(pobjImuCfg, "InstallParam");
    if (pobjInstallParamCfg == NULL) 
        return -1;

    INT32 rslt = ReadInstallParam(pobjInstallParamCfg, "InstallParam", stImuCfg.stInstallPose);
    if (rslt != 0) {
        ST_LOG_ERR("Read Imu Install Pose Param Fail.");    
        return -1;
    }

    ST_LOG_INFO("Cfg Param: <isSupport> = %d.", stImuCfg.isSupport);
    return 0;
}

/**************************************************************************************
功能描述: 读取虚拟的定位激光雷达配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadLocLidarCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_LOC_LIDAR_STRU &stLocLidarCfg)
{
    TiXmlElement *pobjLocLidarCfg = pobjCfgParam->FirstChildElement("LocLidar");
    if (pobjLocLidarCfg == NULL) {
        ST_LOG_ERR("Get <LocLidar> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjLocLidarCfg->QueryBoolAttribute("isSupport", &stLocLidarCfg.isSupport)) {
        ST_LOG_ERR("Get <LocLidar> Cfg Attr <isSupport> Fail.");
        return -1;
    }

    TiXmlElement *pobjInstallParamCfg = GetFirstChildElement(pobjLocLidarCfg, "InstallParam");
    if (pobjInstallParamCfg == NULL) 
        return -1;

    INT32 rslt = ReadInstallParam(pobjInstallParamCfg, "InstallParam", stLocLidarCfg.stInstallPose);
    if (rslt != 0) {
        ST_LOG_ERR("Read LocLidar Install Pose Param Fail.");    
        return -1;
    }

    ST_LOG_INFO("Cfg Param: <isSupport> = %d.", stLocLidarCfg.isSupport);
    return 0;
}

INT32 CFG_PARAM_FILE_C::ReadPlaneFitParam(TiXmlElement *pobjCfgParam, const char *element, CFG_PARAM_PLANE_FIT_STRU &stPlaneFitParam)
{
    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Coeff_y",    stPlaneFitParam.coeffY);
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Coeff_z",    stPlaneFitParam.coeffZ);
    rslt += GetFloatAttributeValue(pobjCfgParam, element, "Const_h",    stPlaneFitParam.constH);

//    ST_LOG_INFO("Coeff_y:%f Coeff_y:%f Coeff_y:%f", stHrzPlaneParam.coeffY, stHrzPlaneParam.coeffZ, stHrzPlaneParam.constH);

    return rslt;
}

/**************************************************************************************
功能描述: 读取车头深度摄像头配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadHeadDepthCameraCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_HEAD_DEPTH_CAMERA_STRU &stHeadDepthCameraCfg)
{
    TiXmlElement *pobjHeadDepthCameraCfg = pobjCfgParam->FirstChildElement("HeadDepthCamera");
    if (pobjHeadDepthCameraCfg == NULL) {
        ST_LOG_ERR("Get <HeadDepthCamera> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjHeadDepthCameraCfg->QueryBoolAttribute("isSupport", &stHeadDepthCameraCfg.isSupport)) {
        ST_LOG_ERR("Get <HeadDepthCamera> Cfg Attr <isSupport> Fail.");
        return -1;
    }

    INT32 rslt = 0;
    
    TiXmlElement *pobjInstallParamCfg = GetFirstChildElement(pobjHeadDepthCameraCfg, "InstallParam");
    if (pobjInstallParamCfg == NULL) return -1;
    rslt = ReadInstallPosParam(pobjInstallParamCfg, "InstallParam", stHeadDepthCameraCfg.stInstallPos);

    TiXmlElement *pobjHrzPlaneParamCfg = GetFirstChildElement(pobjHeadDepthCameraCfg, "HrzPlaneParam");
    if (pobjHrzPlaneParamCfg == NULL) return -1;
    rslt = ReadPlaneFitParam(pobjHrzPlaneParamCfg, "HrzPlaneParam", stHeadDepthCameraCfg.stPlaneFitParam);

    if (rslt != 0) {
        ST_LOG_ERR("Read HeadDepthCamera Config Fail.");    
        return -1;
    }

    ST_LOG_INFO("Cfg Param: <isSupport> = %d.", stHeadDepthCameraCfg.isSupport);
    return 0;
}

/**************************************************************************************
功能描述: 读取车尾深度摄像头配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadBackDepthCameraCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_BACK_DEPTH_CAMERA_STRU &stBackDepthCameraCfg)
{
    TiXmlElement *pobjBackDepthCameraCfg = pobjCfgParam->FirstChildElement("BackDepthCamera");
    if (pobjBackDepthCameraCfg == NULL) {
        ST_LOG_ERR("Get <BackDepthCamera> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjBackDepthCameraCfg->QueryBoolAttribute("isSupport", &stBackDepthCameraCfg.isSupport)) {
        ST_LOG_ERR("Get <BackDepthCamera> Cfg Attr <isSupport> Fail.");
        return -1;
    }

    INT32 rslt = 0;
    
    TiXmlElement *pobjInstallParamCfg = GetFirstChildElement(pobjBackDepthCameraCfg, "InstallParam");
    if (pobjInstallParamCfg == NULL) return -1;
    rslt = ReadInstallPosParam(pobjInstallParamCfg, "InstallParam", stBackDepthCameraCfg.stInstallPos);

    TiXmlElement *pobjHrzPlaneParamCfg = GetFirstChildElement(pobjBackDepthCameraCfg, "HrzPlaneParam");
    if (pobjHrzPlaneParamCfg == NULL) return -1;
    rslt = ReadPlaneFitParam(pobjHrzPlaneParamCfg, "HrzPlaneParam", stBackDepthCameraCfg.stPlaneFitParam);
    
    if (rslt != 0) {
        ST_LOG_ERR("Read BackDepthCamera Config Fail.");    
        return -1;
    }

    ST_LOG_INFO("Cfg Param: <isSupport> = %d.", stBackDepthCameraCfg.isSupport);
    return 0;
}

/**************************************************************************************
功能描述: 读取车左后深度摄像头配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadLeftBackDepthCameraCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_LEFT_BACK_DEPTH_CAMERA_STRU &stLeftBackDepthCameraCfg)
{
    TiXmlElement *pobjLeftBackDepthCameraCfg = pobjCfgParam->FirstChildElement("LeftBackDepthCamera");
    if (pobjLeftBackDepthCameraCfg == NULL) {
        ST_LOG_ERR("Get <LeftBackDepthCamera> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjLeftBackDepthCameraCfg->QueryBoolAttribute("isSupport", &stLeftBackDepthCameraCfg.isSupport)) {
        ST_LOG_ERR("Get <LeftBackDepthCamera> Cfg Attr <isSupport> Fail.");
        return -1;
    }

    TiXmlElement *pobjInstallParamCfg = GetFirstChildElement(pobjLeftBackDepthCameraCfg, "InstallParam");
    if (pobjInstallParamCfg == NULL) return -1;

    INT32 rslt = 0;
    rslt = ReadInstallParam(pobjInstallParamCfg, "InstallParam", stLeftBackDepthCameraCfg.stInstallPose);
    if (rslt != 0) {
        ST_LOG_ERR("Read LeftBackDepthCamera Config Fail.");    
        return -1;
    }

    ST_LOG_INFO("Cfg Param: <isSupport> = %d.", stLeftBackDepthCameraCfg.isSupport);
    return 0;
}

/**************************************************************************************
功能描述: 读取车右后深度摄像头配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadRightBackDepthCameraCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_RIGHT_BACK_DEPTH_CAMERA_STRU &stRightBackDepthCameraCfg)
{
    TiXmlElement *pobjRightBackDepthCameraCfg = pobjCfgParam->FirstChildElement("RightBackDepthCamera");
    if (pobjRightBackDepthCameraCfg == NULL) {
        ST_LOG_ERR("Get <RightBackDepthCamera> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRightBackDepthCameraCfg->QueryBoolAttribute("isSupport", &stRightBackDepthCameraCfg.isSupport)) {
        ST_LOG_ERR("Get <RightBackDepthCamera> Cfg Attr <isSupport> Fail.");
        return -1;
    }

    TiXmlElement *pobjInstallParamCfg = GetFirstChildElement(pobjRightBackDepthCameraCfg, "InstallParam");
    if (pobjInstallParamCfg == NULL) return -1;

    INT32 rslt = 0;
    rslt = ReadInstallParam(pobjInstallParamCfg, "InstallParam", stRightBackDepthCameraCfg.stInstallPose);
    if (rslt != 0) {
        ST_LOG_ERR("Read RightBackDepthCamera Config Fail.");    
        return -1;
    }

    ST_LOG_INFO("Cfg Param: <isSupport> = %d.", stRightBackDepthCameraCfg.isSupport);
    return 0;
}

/**************************************************************************************
功能描述: 读取视觉检测配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadVisionDetectCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_VISION_DETECT_STRU &stVisionDetectCfg)
{
    INT32 rslt = 0;
    
    TiXmlElement *pobjVisionDetectCfg = GetFirstChildElement(pobjCfgParam, "VisionDetect");
    if (pobjVisionDetectCfg == NULL)    return -1;

    /* 读取前广角相机相关参数 */
    TiXmlElement *pobjHeadWideAngleCameraCfg = GetFirstChildElement(pobjVisionDetectCfg, "HeadWideAngleCamera");
    if (pobjHeadWideAngleCameraCfg == NULL) return -1;
    
    rslt += GetBoolAttributeValue(pobjHeadWideAngleCameraCfg, "HeadWideAngleCamera", "isSupport", stVisionDetectCfg.stHeadWideAngleCam.isSupport);
    ST_LOG_INFO("HeadWideAngleCamera: <isSupport> = %d.", stVisionDetectCfg.stHeadWideAngleCam.isSupport);
    
    if (stVisionDetectCfg.stHeadWideAngleCam.isSupport) {
        TiXmlElement *pobjCameraPoseCfg = GetFirstChildElement(pobjHeadWideAngleCameraCfg, "CameraPose");
        if (pobjCameraPoseCfg == NULL)    return -1;
        rslt += ReadInstallParam(pobjCameraPoseCfg, "CameraPose", stVisionDetectCfg.stHeadWideAngleCam.stInstallPose);

        TiXmlElement *pobjDetectPedCarCfg = GetFirstChildElement(pobjHeadWideAngleCameraCfg, "DetectPedCar");
        if (pobjDetectPedCarCfg == NULL)    return -1;
        rslt += ReadDetectObjectParam(pobjDetectPedCarCfg, "DetectPedCar", stVisionDetectCfg.stHeadWideAngleCam.stDetPedCar);

        TiXmlElement *pobjDetectLaneCfg = GetFirstChildElement(pobjHeadWideAngleCameraCfg, "DetectLane");
        if (pobjDetectLaneCfg == NULL)    return -1;
        rslt += ReadDetectObjectParam(pobjDetectLaneCfg, "DetectLane", stVisionDetectCfg.stHeadWideAngleCam.stDetLane);
        ST_LOG_INFO("HeadWideAngleCamera: <DetectPedCar isSupport> = %d, <DetectLane isSupport> = %d.", 
            stVisionDetectCfg.stHeadWideAngleCam.stDetPedCar.isSupport, stVisionDetectCfg.stHeadWideAngleCam.stDetLane.isSupport);
    }

    /* 读取后广角相机相关参数 */
    TiXmlElement *pobjBackWideAngleCameraCfg = GetFirstChildElement(pobjVisionDetectCfg, "BackWideAngleCamera");
    if (pobjBackWideAngleCameraCfg == NULL) return -1;
    
    rslt += GetBoolAttributeValue(pobjBackWideAngleCameraCfg, "BackWideAngleCamera", "isSupport", stVisionDetectCfg.stBackWideAngleCam.isSupport);
    ST_LOG_INFO("BackWideAngleCamera: <isSupport> = %d.", stVisionDetectCfg.stBackWideAngleCam.isSupport);
    
    if (stVisionDetectCfg.stBackWideAngleCam.isSupport) {
        TiXmlElement *pobjCameraPoseCfg = GetFirstChildElement(pobjBackWideAngleCameraCfg, "CameraPose");
        if (pobjCameraPoseCfg == NULL)    return -1;
        rslt += ReadInstallParam(pobjCameraPoseCfg, "CameraPose", stVisionDetectCfg.stBackWideAngleCam.stInstallPose);

        TiXmlElement *pobjDetectPedCarCfg = GetFirstChildElement(pobjBackWideAngleCameraCfg, "DetectPedCar");
        if (pobjDetectPedCarCfg == NULL)    return -1;
        rslt += ReadDetectObjectParam(pobjDetectPedCarCfg, "DetectPedCar", stVisionDetectCfg.stBackWideAngleCam.stDetPedCar);

        TiXmlElement *pobjDetectLaneCfg = GetFirstChildElement(pobjBackWideAngleCameraCfg, "DetectLane");
        if (pobjDetectLaneCfg == NULL)    return -1;
        rslt += ReadDetectObjectParam(pobjDetectLaneCfg, "DetectLane", stVisionDetectCfg.stBackWideAngleCam.stDetLane);
        ST_LOG_INFO("BackWideAngleCamera: <DetectPedCar isSupport> = %d, <DetectLane isSupport> = %d.", 
            stVisionDetectCfg.stBackWideAngleCam.stDetPedCar.isSupport, stVisionDetectCfg.stBackWideAngleCam.stDetLane.isSupport);
    }

    /* 读取前长焦相机相关参数 */
    TiXmlElement *pobjHeadLongFocusCameraCfg = GetFirstChildElement(pobjVisionDetectCfg, "HeadLongFocusCamera");
    if (pobjHeadLongFocusCameraCfg == NULL) return -1;
    
    rslt += GetBoolAttributeValue(pobjHeadLongFocusCameraCfg, "HeadLongFocusCamera", "isSupport", stVisionDetectCfg.stHeadLongFocusCam.isSupport);
    ST_LOG_INFO("HeadLongFocusCamera: <isSupport> = %d.", stVisionDetectCfg.stHeadLongFocusCam.isSupport);
    
    if (stVisionDetectCfg.stHeadLongFocusCam.isSupport) {
        TiXmlElement *pobjCameraPoseCfg = GetFirstChildElement(pobjHeadLongFocusCameraCfg, "CameraPose");
        if (pobjCameraPoseCfg == NULL)    return -1;
        rslt += ReadInstallParam(pobjCameraPoseCfg, "CameraPose", stVisionDetectCfg.stHeadLongFocusCam.stInstallPose);

        TiXmlElement *pobjDetectTrafficLightCfg = GetFirstChildElement(pobjHeadLongFocusCameraCfg, "DetectTrafficLight");
        if (pobjDetectTrafficLightCfg == NULL)    return -1;
        rslt += ReadDetectObjectParam(pobjDetectTrafficLightCfg, "DetectTrafficLight", stVisionDetectCfg.stHeadLongFocusCam.stDetTrafficLight);
        ST_LOG_INFO("HeadLongFocusCamera: <DetectTrafficLight isSupport> = %d.", 
            stVisionDetectCfg.stHeadLongFocusCam.stDetTrafficLight.isSupport);
    }
    
    return rslt;
}

/**************************************************************************************
功能描述: 读取所有激光雷达配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadLidarCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_LIDAR_STRU &stLidarCfg)
{
    TiXmlElement *pobjLidarCfg = pobjCfgParam->FirstChildElement("LidarGroup");
    if (pobjLidarCfg == NULL) {
        ST_LOG_ERR("Get <LidarGroup> Cfg Element Fail.");
        return -1;
    }

    TiXmlElement *pobjHeadLeftRs16LidarCfg = pobjLidarCfg->FirstChildElement("HeadLeftRs16Lidar");
    if (ReadHeadLidarCfg(pobjHeadLeftRs16LidarCfg, stLidarCfg.stHeadLeftRs16Lidar) != 0) {
        ST_LOG_ERR("Get <HeadLeftRs16Lidar> Cfg Element Fail.");
        return -1;
    }
    else{
        ST_LOG_INFO("HeadLeftRs16Lidar: <isSupport> = %d.", stLidarCfg.stHeadLeftRs16Lidar.isSupport);
    }

    TiXmlElement *pobjHeadLeftRsBpearlLidarCfg = pobjLidarCfg->FirstChildElement("HeadLeftRsBpearlLidar");
    if (ReadHeadLidarCfg(pobjHeadLeftRsBpearlLidarCfg, stLidarCfg.stHeadLeftRsBpearlLidar) != 0) {
        ST_LOG_ERR("Get <HeadLeftRsBpearlLidar> Cfg Element Fail.");
        return -1;
    }
    else{
        ST_LOG_INFO("HeadLeftRsBpearlLidar: <isSupport> = %d.", stLidarCfg.stHeadLeftRsBpearlLidar.isSupport);
    }

    TiXmlElement *pobjHeadRightRsBpearlLidarCfg = pobjLidarCfg->FirstChildElement("HeadRightRsBpearlLidar");
    if (ReadHeadLidarCfg(pobjHeadRightRsBpearlLidarCfg, stLidarCfg.stHeadRightRsBpearlLidar) != 0) {
        ST_LOG_ERR("Get <HeadRightRsBpearlLidar> Cfg Element Fail.");
        return -1;
    }
    else{
        ST_LOG_INFO("HeadRightRsBpearlLidar: <isSupport> = %d.", stLidarCfg.stHeadRightRsBpearlLidar.isSupport);
    }

    TiXmlElement *pobjMiddleRs16LidarCfg = pobjLidarCfg->FirstChildElement("MiddleRs16Lidar");
    if (ReadMiddleLidarCfg(pobjMiddleRs16LidarCfg, stLidarCfg.stMiddleRs16Lidar) != 0) {
        ST_LOG_ERR("Get <MiddleRs16Lidar> Cfg Element Fail.");
        return -1;
    }
    else{
        ST_LOG_INFO("MiddleRs16Lidar: <isSupport> = %d.", stLidarCfg.stMiddleRs16Lidar.isSupport);
    }

    TiXmlElement *pobjMiddleRsBpearlLidarCfg = pobjLidarCfg->FirstChildElement("MiddleRsBpearlLidar");
    if (ReadMiddleLidarCfg(pobjMiddleRsBpearlLidarCfg, stLidarCfg.stMiddleRsBpearlLidar) != 0) {
        ST_LOG_ERR("Get <MiddleRsBpearlLidar> Cfg Element Fail.");
        return -1;
    }
    else{
        ST_LOG_INFO("MiddleRsBpearlLidar: <isSupport> = %d.", stLidarCfg.stMiddleRsBpearlLidar.isSupport);
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取安装在车头的激光雷达配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadHeadLidarCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_HEAD_LIDAR_STRU &stHeadLidarCfg)
{
     if (pobjCfgParam == NULL) {
        ST_LOG_ERR("Get Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryBoolAttribute("isSupport", &stHeadLidarCfg.isSupport)) {
        ST_LOG_ERR("Get Cfg Attr <isSupport> Fail.");
        return -1;
    }
    if(!stHeadLidarCfg.isSupport)  return 0;
    
    TiXmlElement *pobjRsLidarDriverCfg = pobjCfgParam->FirstChildElement("DriverParam");
    if (ReadRsLidarDriverCfg(pobjRsLidarDriverCfg, stHeadLidarCfg.stDriverParam) != 0) {
        ST_LOG_ERR("Rs Lidar Driver Cfg Param Error!");
        return -1;
    }
    
    TiXmlElement *pobjRsLidarInstallCfg = pobjCfgParam->FirstChildElement("InstallParam");
    if (ReadRsLidarInstallCfg(pobjRsLidarInstallCfg, stHeadLidarCfg.stInstallParam) != 0) {
        ST_LOG_ERR("Rs Lidar Install Cfg Param Error!");
        return -1;
    }

    TiXmlElement *pobjRsLidarCarFilterCfg = pobjCfgParam->FirstChildElement("CarFilterParam");
    if (ReadHeadRsLidarCarFilterCfg(pobjRsLidarCarFilterCfg, stHeadLidarCfg.stCarFilterParam) != 0) {
        ST_LOG_ERR("Rs Lidar Car Filter Cfg Param Error!");
        return -1;
    }

    TiXmlElement *pobjRsLidarSectionWipeCfg = pobjCfgParam->FirstChildElement("LaserSectionWipe");
    if (ReadRsLidarSectionWipeCfg(pobjRsLidarSectionWipeCfg, "LaserSectionWipe", stHeadLidarCfg.stLidarSectionWipe) != 0) {
        ST_LOG_ERR("Rs Lidar Laser Section Wipe Cfg Param Error!");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取安装在车中间的激光雷达配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadMiddleLidarCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_MIDDLE_LIDAR_STRU &stMiddleLidarCfg)
{
    if (pobjCfgParam == NULL) {
        ST_LOG_ERR("Get Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryBoolAttribute("isSupport", &stMiddleLidarCfg.isSupport)) {
        ST_LOG_ERR("Get Cfg Attr <isSupport> Fail.");
        return -1;
    }
    if(!stMiddleLidarCfg.isSupport)  return 0;
    
    TiXmlElement *pobjRsLidarDriverCfg = pobjCfgParam->FirstChildElement("DriverParam");
    if (ReadRsLidarDriverCfg(pobjRsLidarDriverCfg, stMiddleLidarCfg.stDriverParam) != 0) {
        ST_LOG_ERR("Rs Lidar Driver Cfg Param Error!");
        return -1;
    }
    
    TiXmlElement *pobjRsLidarInstallCfg = pobjCfgParam->FirstChildElement("InstallParam");
    if (ReadRsLidarInstallCfg(pobjRsLidarInstallCfg, stMiddleLidarCfg.stInstallParam) != 0) {
        ST_LOG_ERR("Rs Lidar Install Cfg Param Error!");
        return -1;
    }

    TiXmlElement *pobjRsLidarCarFilterCfg = pobjCfgParam->FirstChildElement("CarFilterParam");
    if (ReadMiddleRsLidarCarFilterCfg(pobjRsLidarCarFilterCfg, stMiddleLidarCfg.stCarFilterParam) != 0) {
        ST_LOG_ERR("Rs Lidar Car Filter Cfg Param Error!");
        return -1;
    }

    TiXmlElement *pobjRsLidarSectionWipeCfg = pobjCfgParam->FirstChildElement("LaserSectionWipe");
    if (ReadRsLidarSectionWipeCfg(pobjRsLidarSectionWipeCfg, "LaserSectionWipe", stMiddleLidarCfg.stLidarSectionWipe) != 0) {
        ST_LOG_ERR("Rs Lidar Laser Section Wipe Cfg Param Error!");
        return -1;
    }
    
    stMiddleLidarCfg.stCarFilterParam.stCarFilterParam.vertex.x 
        = stMiddleLidarCfg.stInstallParam.tx;
    stMiddleLidarCfg.stCarFilterParam.stCarFilterParam.vertex.y 
        = stMiddleLidarCfg.stInstallParam.ty;
    stMiddleLidarCfg.stCarFilterParam.stCarFilterParam.vertex.z 
        = stMiddleLidarCfg.stInstallParam.tz;

    return 0;
}

/**************************************************************************************
功能描述: 读取激光雷达驱动配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadRsLidarDriverCfg(TiXmlElement *pobjRsLidarDriverCfg, CFG_PARAM_RS_LIDAR_DRIVER_STRU &stDriverCfg)
{
    if(pobjRsLidarDriverCfg == NULL) {
        ST_LOG_ERR("Get <DriverParam> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDriverCfg->QueryStringAttribute("Model", &stDriverCfg.strModel)) {
        ST_LOG_ERR("Get <DriverParam> Cfg Attr <Model> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDriverCfg->QueryStringAttribute("FrameId", &stDriverCfg.strFrameId)) {
        ST_LOG_ERR("Get <DriverParam> Cfg Attr <FrameId> Fail.");
        return -1;
    }

    TiXmlElement *pobjRsLidarDevCfg = pobjRsLidarDriverCfg->FirstChildElement("DevParam");
    if (ReadRsLidarDevCfg(pobjRsLidarDevCfg, stDriverCfg.stDevParam) != 0) {
        ST_LOG_ERR("Rs Lidar Dev Cfg Param Error!");
        return -1;
    }

    TiXmlElement *pobjRsLidarDataCfg = pobjRsLidarDriverCfg->FirstChildElement("DataParam");
    if (ReadRsLidarDataCfg(pobjRsLidarDataCfg, stDriverCfg.stDataParam) != 0) {
        ST_LOG_ERR("Rs Lidar Data Cfg Param Error!");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取RS雷达驱动设备配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadRsLidarDevCfg(TiXmlElement *pobjRsLidarDevCfg, CFG_PARAM_RS_LIDAR_DEV_STRU &stDevCfg)
{
    if(pobjRsLidarDevCfg == NULL) {
        ST_LOG_ERR("Get <DevParam> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDevCfg->QueryStringAttribute("Ip", &stDevCfg.strIp)) {
        ST_LOG_ERR("Get <DevParam> Cfg Attr <Ip> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDevCfg->QueryIntAttribute("MsopPort", &stDevCfg.msopPort)) {
        ST_LOG_ERR("Get <DevParam> Cfg Attr <MsopPort> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDevCfg->QueryIntAttribute("DifopPort", &stDevCfg.difopPort)) {
        ST_LOG_ERR("Get <DevParam> Cfg Attr <DifopPort> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDevCfg->QueryFloatAttribute("CutAngle", &stDevCfg.cutAngle)) {
        ST_LOG_ERR("Get <DevParam> Cfg Attr <CutAngle> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDevCfg->QueryFloatAttribute("Rpm", &stDevCfg.rpm)) {
        ST_LOG_ERR("Get <DevParam> Cfg Attr <Rpm> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDevCfg->QueryBoolAttribute("isTimeSync", &stDevCfg.isTimeSync)) {
        ST_LOG_ERR("Get <DevParam> Cfg Attr <isTimeSync> Fail.");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取RS雷达驱动数据解析配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadRsLidarDataCfg(TiXmlElement *pobjRsLidarDataCfg, CFG_PARAM_RS_LIDAR_DATA_STRU &stDataCfg)
{
    if(pobjRsLidarDataCfg == NULL) {
        ST_LOG_ERR("Get <DataParam> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDataCfg->QueryIntAttribute("StartAngle", &stDataCfg.startAngle)) {
        ST_LOG_ERR("Get <DataParam> Cfg Attr <StartAngle> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDataCfg->QueryIntAttribute("EndAngle", &stDataCfg.endAngle)) {
        ST_LOG_ERR("Get <DataParam> Cfg Attr <EndAngle> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDataCfg->QueryFloatAttribute("MinDist", &stDataCfg.minDist)) {
        ST_LOG_ERR("Get <DataParam> Cfg Attr <MinDist> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDataCfg->QueryFloatAttribute("MaxDist", &stDataCfg.maxDist)) {
        ST_LOG_ERR("Get <DataParam> Cfg Attr <MaxDist> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDataCfg->QueryStringAttribute("Resolution", &stDataCfg.resolution)) {
        ST_LOG_ERR("Get <DataParam> Cfg Attr <Resolution> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarDataCfg->QueryIntAttribute("IntensityMode", &stDataCfg.intensityMode)) {
        ST_LOG_ERR("Get <DataParam> Cfg Attr <IntensityMode> Fail.");
        return -1;
    }

    FILE_MNG_C objFileMng;
    string strLidarConfigDir;
    objFileMng.GetLidarConfigDir(strLidarConfigDir);

    if (TIXML_SUCCESS != pobjRsLidarDataCfg->QueryStringAttribute("AngleFileName", &stDataCfg.angleFileName)) {
        ST_LOG_ERR("Get <DataParam> Cfg Attr <AngleFileName> Fail.");
        return -1;
    }
    stDataCfg.angleFileName = strLidarConfigDir + stDataCfg.angleFileName;

    if (TIXML_SUCCESS != pobjRsLidarDataCfg->QueryStringAttribute("ChannelFileName", &stDataCfg.channelFileName)) {
        ST_LOG_ERR("Get <DataParam> Cfg Attr <ChannelFileName> Fail.");
        return -1;
    }
    stDataCfg.channelFileName = strLidarConfigDir + stDataCfg.channelFileName;

    if (TIXML_SUCCESS != pobjRsLidarDataCfg->QueryStringAttribute("CurvesFileName", &stDataCfg.curvesFileName)) {
        ST_LOG_ERR("Get <DataParam> Cfg Attr <CurvesFileName> Fail.");
        return -1;
    }
    stDataCfg.curvesFileName = strLidarConfigDir + stDataCfg.curvesFileName;

    return 0;
}

/**************************************************************************************
功能描述: 读取RS雷达安装位姿配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadRsLidarInstallCfg(TiXmlElement *pobjRsLidarInstallCfg, CFG_PARAM_INSTALL_STRU &stInstallCfg)
{
    if(pobjRsLidarInstallCfg == NULL) {
        ST_LOG_ERR("Get <InstallParam> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarInstallCfg->QueryFloatAttribute("Tx", &stInstallCfg.tx)) {
        ST_LOG_ERR("Get <InstallParam> Cfg Attr <Tx> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarInstallCfg->QueryFloatAttribute("Ty", &stInstallCfg.ty)) {
        ST_LOG_ERR("Get <InstallParam> Cfg Attr <Ty> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarInstallCfg->QueryFloatAttribute("Tz", &stInstallCfg.tz)) {
        ST_LOG_ERR("Get <InstallParam> Cfg Attr <Tz> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarInstallCfg->QueryFloatAttribute("Roll", &stInstallCfg.roll)) {
        ST_LOG_ERR("Get <InstallParam> Cfg Attr <Roll> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarInstallCfg->QueryFloatAttribute("Pitch", &stInstallCfg.pitch)) {
        ST_LOG_ERR("Get <InstallParam> Cfg Attr <Pitch> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjRsLidarInstallCfg->QueryFloatAttribute("Yaw", &stInstallCfg.yaw)) {
        ST_LOG_ERR("Get <InstallParam> Cfg Attr <Yaw> Fail.");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取安装在车头的激光雷达车身点云过滤参数配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadHeadRsLidarCarFilterCfg(TiXmlElement *pobjRsLidarCarFilterCfg, CFG_PARAM_HEAD_LIDAR_CAR_FILTER_STRU &stCarFilterCfg)
{
    if(pobjRsLidarCarFilterCfg == NULL) {
        ST_LOG_ERR("Get <CarFilterParam> Cfg Element Fail.");
        return -1;
    }

    TiXmlElement *pobjRsLidarCarHeadFilterCfg = pobjRsLidarCarFilterCfg->FirstChildElement("CarHeadParam");
    if (ReadTriangularPrismCfg(pobjRsLidarCarHeadFilterCfg, stCarFilterCfg.stCarHeadFilterParam) != 0) {
        ST_LOG_ERR("Get <CarHeadParam> Cfg Element Fail.");
        return -1;
    }

    TiXmlElement *pobjRsLidarCarTailtopFilterCfg = pobjRsLidarCarFilterCfg->FirstChildElement("CarTailTopParam");
    if (ReadCuboidCfg(pobjRsLidarCarTailtopFilterCfg, stCarFilterCfg.stCarTailtopFilterParam) != 0) {
        ST_LOG_ERR("Get <CarTailTopParam> Cfg Element Fail.");
        return -1;
    }

    TiXmlElement *pobjRsLidarCarTailbottomFilterCfg = pobjRsLidarCarFilterCfg->FirstChildElement("CarTailBottomParam");
    if (ReadCuboidCfg(pobjRsLidarCarTailbottomFilterCfg, stCarFilterCfg.stCarTailbottomFilterParam) != 0) {
        ST_LOG_ERR("Get <CarTailBottomParam> Cfg Element Fail.");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取安装在车中间的激光雷达车身点云过滤参数配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadMiddleRsLidarCarFilterCfg(TiXmlElement *pobjRsLidarCarFilterCfg, CFG_PARAM_MIDDLE_LIDAR_CAR_FILTER_STRU &stCarFilterCfg)
{
    if(pobjRsLidarCarFilterCfg == NULL) {
        ST_LOG_ERR("Get <CarFilterParam> Cfg Element Fail.");
        return -1;
    }

    TiXmlElement *pobjRsLidarCarCfg = pobjRsLidarCarFilterCfg->FirstChildElement("CarParam");
    if (ReadPyramidCfg(pobjRsLidarCarCfg, stCarFilterCfg.stCarFilterParam) != 0) {
        ST_LOG_ERR("Rs Lidar CarParam Cfg Param Error!");
        return -1;
    }

    TiXmlElement *pobjRsLidarRackCfg = pobjRsLidarCarFilterCfg->FirstChildElement("LidarRackParam");
    if (ReadCuboidCfg(pobjRsLidarRackCfg, stCarFilterCfg.stLidarRackFilterParam) != 0) {
        ST_LOG_ERR("Rs Lidar Rack Cfg Param Error!");
        return -1;
    }

    return 0;
}

INT32 CFG_PARAM_FILE_C::ReadLaserSectionWipeCfg(TiXmlElement *pobjSection, const char *element, CFG_PARAM_LASER_SECTION_WIPE_STRU &stLaserSectionWipe)
{
    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(pobjSection, element, "StartAngle",  stLaserSectionWipe.startAngle);
    rslt += GetFloatAttributeValue(pobjSection, element, "EndAngle",    stLaserSectionWipe.endAngle);
    if (stLaserSectionWipe.startAngle > stLaserSectionWipe.endAngle) {
        ST_LOG_ERR("Section <%f~%f> Error. Please Set the Correct Begin-End Angle. Note: StartAngle <= EndAngle", stLaserSectionWipe.startAngle, stLaserSectionWipe.endAngle);
        return -1;
    }

//    ST_LOG_INFO("Laser Wipe Section:%f %f", stLaserSectionWipe.startAngle, stLaserSectionWipe.endAngle);
    return rslt;
}

/**************************************************************************************
功能描述: 读取安装在车中间的激光雷达激光扇区去除参数配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadRsLidarSectionWipeCfg(TiXmlElement *pobjRsLidarSectionWipeCfg, const char *element, CFG_PARAM_LIDAR_SECTION_WIPE_STRU &stLidarSectionWipe)
{
    if (pobjRsLidarSectionWipeCfg == NULL) {
        ST_LOG_ERR("Get <LaserSectionWipe> Cfg Element Fail.");
        return -1;
    }
    
    INT32 rslt = GetBoolAttributeValue(pobjRsLidarSectionWipeCfg, element, "isSupport", stLidarSectionWipe.isSupport);
    
    if (rslt == 0 && stLidarSectionWipe.isSupport) {
        TiXmlElement *pobjSection = GetFirstChildElement(pobjRsLidarSectionWipeCfg, "Section");
        if (pobjSection == NULL)    return -1;
        CFG_PARAM_LASER_SECTION_WIPE_STRU stLaserSectionWipe;
        for(; pobjSection != NULL; pobjSection = pobjSection->NextSiblingElement()) {
            rslt += ReadLaserSectionWipeCfg(pobjSection, "Section", stLaserSectionWipe);
            stLidarSectionWipe.vstLaserSectionWipe.push_back(stLaserSectionWipe);
        }
    }
    
    return rslt;
}

/**************************************************************************************
功能描述: 读取三棱柱参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadTriangularPrismCfg(TiXmlElement *pobjTriangularPrismCfg, TRI_PRISM_STRU &stTriangularPrism)
{
    if(pobjTriangularPrismCfg == NULL) {
        ST_LOG_ERR("Get TriangularPrism Cfg Element Fail.");
        return -1;
    }
    
    float minX, maxX, minY, maxY, minZ, maxZ;
    if (TIXML_SUCCESS != pobjTriangularPrismCfg->QueryFloatAttribute("MinX", &minX)) {
        ST_LOG_ERR("Get TriangularPrism Cfg Attr <MinX> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjTriangularPrismCfg->QueryFloatAttribute("MaxX", &maxX)) {
        ST_LOG_ERR("Get TriangularPrism Cfg Attr <MaxX> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjTriangularPrismCfg->QueryFloatAttribute("MinY", &minY)) {
        ST_LOG_ERR("Get TriangularPrism Cfg Attr <MinY> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjTriangularPrismCfg->QueryFloatAttribute("MaxY", &maxY)) {
        ST_LOG_ERR("Get TriangularPrism Cfg Attr <MaxY> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjTriangularPrismCfg->QueryFloatAttribute("MinZ", &minZ)) {
        ST_LOG_ERR("Get TriangularPrism Cfg Attr <MinZ> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjTriangularPrismCfg->QueryFloatAttribute("MaxZ", &maxZ)) {
        ST_LOG_ERR("Get TriangularPrism Cfg Attr <MaxZ> Fail.");
        return -1;
    }

    stTriangularPrism.SetValue(minX, maxX, minY, maxY, minZ, maxZ);

    return 0;
}

/**************************************************************************************
功能描述: 读取长方体参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadCuboidCfg(TiXmlElement *pobjCuboidCfg, CUBOID_STRU &stCuboid)
{
    if(pobjCuboidCfg == NULL) {
        ST_LOG_ERR("Get Cuboid Cfg Element Fail.");
        return -1;
    }

    float minX, maxX, minY, maxY, minZ, maxZ;
    if (TIXML_SUCCESS != pobjCuboidCfg->QueryFloatAttribute("MinX", &minX)) {
        ST_LOG_ERR("Get Cuboid Cfg Attr <MinX> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCuboidCfg->QueryFloatAttribute("MaxX", &maxX)) {
        ST_LOG_ERR("Get Cuboid Cfg Attr <MaxX> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCuboidCfg->QueryFloatAttribute("MinY", &minY)) {
        ST_LOG_ERR("Get Cuboid Cfg Attr <MinY> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCuboidCfg->QueryFloatAttribute("MaxY", &maxY)) {
        ST_LOG_ERR("Get Cuboid Cfg Attr <MaxY> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCuboidCfg->QueryFloatAttribute("MinZ", &minZ)) {
        ST_LOG_ERR("Get Cuboid Cfg Attr <MinZ> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCuboidCfg->QueryFloatAttribute("MaxZ", &maxZ)) {
        ST_LOG_ERR("Get Cuboid Cfg Attr <MaxZ> Fail.");
        return -1;
    }
    
    stCuboid.SetValue(minX, maxX, minY, maxY, minZ, maxZ);

    return 0;
}

/**************************************************************************************
功能描述: 读取四棱锥参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPyramidCfg(TiXmlElement *pobjPyramidCfg, PYRAMID_STRU &stPyramid)
{
    if(pobjPyramidCfg == NULL) {
        ST_LOG_ERR("Get Pyramid Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjPyramidCfg->QueryFloatAttribute("MinX", &stPyramid.minX)) {
        ST_LOG_ERR("Get Pyramid Cfg Attr <MinX> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjPyramidCfg->QueryFloatAttribute("MaxX", &stPyramid.maxX)) {
        ST_LOG_ERR("Get Pyramid Cfg Attr <MaxX> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjPyramidCfg->QueryFloatAttribute("MinY", &stPyramid.minY)) {
        ST_LOG_ERR("Get Pyramid Cfg Attr <MinY> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjPyramidCfg->QueryFloatAttribute("MaxY", &stPyramid.maxY)) {
        ST_LOG_ERR("Get Pyramid Cfg Attr <MaxY> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjPyramidCfg->QueryFloatAttribute("VertexX", &stPyramid.vertex.x)) {
        ST_LOG_ERR("Get Pyramid Cfg Attr <VertexX> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjPyramidCfg->QueryFloatAttribute("VertexY", &stPyramid.vertex.y)) {
        ST_LOG_ERR("Get Pyramid Cfg Attr <VertexY> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjPyramidCfg->QueryFloatAttribute("VertexZ", &stPyramid.vertex.z)) {
        ST_LOG_ERR("Get Pyramid Cfg Attr <VertexZ> Fail.");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取多激光雷达融合
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadMultiLidarFusionCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_MULTI_LIDAR_FUSION_STRU &stMultiLidarFusionCfg)
{
    TiXmlElement *pobjMultiLidarFusionCfg = pobjCfgParam->FirstChildElement("MultiLidarFusion");
    if (pobjMultiLidarFusionCfg == NULL) {
        ST_LOG_ERR("Get <MultiLidarFusion> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjMultiLidarFusionCfg->QueryBoolAttribute("isUseHeadLeftRs16Lidar", &stMultiLidarFusionCfg.isUseHeadLeftRs16Lidar)) {
        ST_LOG_ERR("Get <MultiLidarFusion> Cfg Attr <isUseHeadLeftRs16Lidar> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjMultiLidarFusionCfg->QueryBoolAttribute("isUseHeadLeftRsBpearlLidar", &stMultiLidarFusionCfg.isUseHeadLeftRsBpearlLidar)) {
        ST_LOG_ERR("Get <MultiLidarFusion> Cfg Attr <isUseHeadLeftRsBpearlLidar> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjMultiLidarFusionCfg->QueryBoolAttribute("isUseHeadRightRsBpearlLidar", &stMultiLidarFusionCfg.isUseHeadRightRsBpearlLidar)) {
        ST_LOG_ERR("Get <MultiLidarFusion> Cfg Attr <isUseHeadRightRsBpearlLidar> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjMultiLidarFusionCfg->QueryBoolAttribute("isUseMiddleRs16Lidar", &stMultiLidarFusionCfg.isUseMiddleRs16Lidar)) {
        ST_LOG_ERR("Get <MultiLidarFusion> Cfg Attr <isUseMiddleRs16Lidar> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjMultiLidarFusionCfg->QueryBoolAttribute("isUseMiddleRsBpearlLidar", &stMultiLidarFusionCfg.isUseMiddleRsBpearlLidar)) {
        ST_LOG_ERR("Get <MultiLidarFusion> Cfg Attr <isUseMiddleRsBpearlLidar> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjMultiLidarFusionCfg->QueryBoolAttribute("isUndistort", &stMultiLidarFusionCfg.isUndistort)) {
        ST_LOG_ERR("Get <MultiLidarFusion> Cfg Attr <isUndistort> Fail.");
        return -1;
    }
    
    ST_LOG_INFO("Cfg Param: <isUseHeadLeftRs16Lidar>  = %d.",     stMultiLidarFusionCfg.isUseHeadLeftRs16Lidar);
    ST_LOG_INFO("Cfg Param: <isUseHeadLeftRsBpearlLidar>  = %d.", stMultiLidarFusionCfg.isUseHeadLeftRsBpearlLidar);
    ST_LOG_INFO("Cfg Param: <isUseHeadRightRsBpearlLidar> = %d.", stMultiLidarFusionCfg.isUseHeadRightRsBpearlLidar);
    ST_LOG_INFO("Cfg Param: <isUseMiddleRs16Lidar>  = %d.",       stMultiLidarFusionCfg.isUseMiddleRs16Lidar);
    ST_LOG_INFO("Cfg Param: <isUseMiddleRsBpearlLidar>  = %d.",   stMultiLidarFusionCfg.isUseMiddleRsBpearlLidar);
    ST_LOG_INFO("Cfg Param: <isUndistort>  = %d.",                stMultiLidarFusionCfg.isUndistort);
    return 0;
}

/**************************************************************************************
功能描述: 读取定位参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadLocalizationCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_LOC_STRU &stLocCfg)
{
    TiXmlElement *pobjLocCfg = pobjCfgParam->FirstChildElement("Localization");
    if (pobjLocCfg == NULL) {
        ST_LOG_ERR("Get <Localization> Cfg Element Fail.");
        return -1;
    }

    TiXmlElement *pobjPoseEstimatorCfg = pobjLocCfg->FirstChildElement("PoseEstimator");
    if (ReadPoseEstimatorCfg(pobjPoseEstimatorCfg, stLocCfg.stPoseEstimator) != 0){
        ST_LOG_ERR("Pose Estimator Cfg Param Error!");
        return -1;
    }

    TiXmlElement *pobjLidarLocCfg = pobjLocCfg->FirstChildElement("LidarLoc");
    if (ReadLidarLocCfg(pobjLidarLocCfg, stLocCfg.stLidarLoc) != 0){
        ST_LOG_ERR("Lidar Loc Cfg Param Error!");
        return -1;
    }

    TiXmlElement *pobjReLocCfg = pobjLocCfg->FirstChildElement("Reloc");
    if (ReadReLocalizationCfg(pobjReLocCfg, stLocCfg.stReloc) != 0) {
        ST_LOG_ERR("Reloc Cfg Param Error!");
        return -1;
    }
    
    return 0;
}

/**************************************************************************************
功能描述: 读取位姿估计器参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPoseEstimatorCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_POSE_ESTIMATOR_STRU &stPoseEstimator)
{
    if (pobjCfgParam == NULL) {
        ST_LOG_ERR("Get <PoseEstimator> Cfg Element Fail.");
        return -1;
    }

    INT32 fusionMethod;
    if (TIXML_SUCCESS != pobjCfgParam->QueryIntAttribute("FusionMethod", &fusionMethod)) {
        ST_LOG_ERR("Get <PoseEstimator> Cfg Attr <FusionMethod> Fail.");
        return -1;
    }
    stPoseEstimator.enFusionMethod = (FUSION_METHOD_ENUM)fusionMethod;
    ST_LOG_INFO("Pose Estimator Fusion Method %d.", (INT32)stPoseEstimator.enFusionMethod);
    
    return 0;
}

/**************************************************************************************
功能描述: 读取激光雷达定位参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadLidarLocCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_LIDAR_LOC_STRU &stLidarLocCfg)
{
    if (pobjCfgParam == NULL) {
        ST_LOG_ERR("Get <LidarLoc> Cfg Element Fail.");
        return -1;
    }

    INT32 matchMethod;
    if (TIXML_SUCCESS != pobjCfgParam->QueryIntAttribute("MatchType", &matchMethod)) {
        ST_LOG_ERR("Get <LidarLoc> Cfg Attr <MatchMethod> Fail.");
        return -1;
    }
    stLidarLocCfg.enMatchMethod = (MATCH_METHOD_ENUM)matchMethod;

    TiXmlElement *pobjMatcherCfg = NULL;
    switch (stLidarLocCfg.enMatchMethod)
    {
        case NDT:
            pobjMatcherCfg = pobjCfgParam->FirstChildElement("NDT");
            if (ReadNdtMatcherCfg(pobjMatcherCfg, stLidarLocCfg.stNdtMatcher) != 0){
                ST_LOG_ERR("Ndt Matcher Cfg Param Error!");
                return -1;
            }
            break;
        default:
            ST_LOG_ERR("Unknow Match Method.");
            return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取Ndt匹配算法参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadNdtMatcherCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_NDT_MATCHER_STRU &stNdtMatcherCfg)
{
    if (pobjCfgParam == NULL) {
        ST_LOG_ERR("Get <NDT> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryIntAttribute("MaxIter", &stNdtMatcherCfg.maxIter)) {
        ST_LOG_ERR("Get <NdtMatcher> Cfg Attr <MaxIter> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("StepSize", &stNdtMatcherCfg.stepSize)) {
        ST_LOG_ERR("Get <NdtMatcher> Cfg Attr <StepSize> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("Resolution", &stNdtMatcherCfg.resolution)) {
        ST_LOG_ERR("Get <NdtMatcher> Cfg Attr <Resolution> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("TransEps", &stNdtMatcherCfg.transEps)) {
        ST_LOG_ERR("Get <NdtMatcher> Cfg Attr <TransEps> Fail.");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取重定位参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadReLocalizationCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_RELOC_STRU &stReLocCfg)
{
    if (pobjCfgParam == NULL) {
        ST_LOG_ERR("Get <Relocalization> Cfg Element Fail.");
        return -1;
    }

    TiXmlElement *pobjSearchRegionCfg = pobjCfgParam->FirstChildElement("SearchRegion");
    if (ReadSearchRegionCfg(pobjSearchRegionCfg, stReLocCfg.stSearchRegion) != 0) {
        ST_LOG_ERR("Search Region Cfg Param Error!");
        return -1;
    }

    TiXmlElement *pobjSearchStepCfg = pobjCfgParam->FirstChildElement("SearchStep");
    if (ReadSearchStepCfg(pobjSearchStepCfg, stReLocCfg.stSearchStep) != 0) {
        ST_LOG_ERR("Search Step Cfg Param Error!");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取重定位搜索区域参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadSearchRegionCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_SEARCH_REG_STRU &stSearchRegCfg)
{
    if (pobjCfgParam == NULL) {
        ST_LOG_ERR("Get <SearchRegion> Cfg Element Fail.");
        return -1;
    } 

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("MinX", &stSearchRegCfg.minX)) {
        ST_LOG_ERR("Get <SearchRegion> Cfg Attr <MinX> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("MaxX", &stSearchRegCfg.maxX)) {
        ST_LOG_ERR("Get <SearchRegion> Cfg Attr <MaxX> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("MinY", &stSearchRegCfg.minY)) {
        ST_LOG_ERR("Get <SearchRegion> Cfg Attr <MinY> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("MaxY", &stSearchRegCfg.maxY)) {
        ST_LOG_ERR("Get <SearchRegion> Cfg Attr <MaxY> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("MinYaw", &stSearchRegCfg.minYaw)) {
        ST_LOG_ERR("Get <SearchRegion> Cfg Attr <MinYaw> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("MaxYaw", &stSearchRegCfg.maxYaw)) {
        ST_LOG_ERR("Get <SearchRegion> Cfg Attr <MaxYaw> Fail.");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取重定位搜索步长参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadSearchStepCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_SEARCH_STEP_STRU &stSearchStepCfg)
{
    if (pobjCfgParam == NULL) {
        ST_LOG_ERR("Get <SearchStep> Cfg Element Fail.");
        return -1;
    } 

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("StepX", &stSearchStepCfg.stepX)) {
        ST_LOG_ERR("Get <SearchStep> Cfg Attr <StepX> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("StepY", &stSearchStepCfg.stepY)) {
        ST_LOG_ERR("Get <SearchStep> Cfg Attr <StepY> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjCfgParam->QueryFloatAttribute("StepYaw", &stSearchStepCfg.stepYaw)) {
        ST_LOG_ERR("Get <SearchStep> Cfg Attr <StepYaw> Fail.");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 读取毫米波雷达配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C:: ReadRadarDevCfg(TiXmlElement *pobjCfgParam, const char *element, CFG_PARAM_RADAR_DEV_STRU &stRadarDevCfg)
{
    INT32 rslt = 0;

    rslt += GetBoolAttributeValue(pobjCfgParam, element, "isSupport", stRadarDevCfg.isSupport);
    ST_LOG_INFO("%s Cfg Param: <isSupport> = %d.", element, stRadarDevCfg.isSupport);
    if (!stRadarDevCfg.isSupport)
        return 0;
    
    TiXmlElement *pobjMaxObstCountCfg = GetFirstChildElement(pobjCfgParam, "MaxObstCount");
    if(pobjMaxObstCountCfg == NULL)     return -1;
    rslt += GetUnsignedAttributeValue(pobjMaxObstCountCfg, "MaxObstCount", "Num", stRadarDevCfg.stDetectParam.maxObstCount);

    TiXmlElement *pobjCollDetRegCfg = GetFirstChildElement(pobjCfgParam, "CollDetReg");
    if (pobjCollDetRegCfg == NULL)      return -1;
    rslt += GetFloatAttributeValue(pobjCollDetRegCfg, "CollDetReg", "RightBottomPoint_X", stRadarDevCfg.stDetectParam.rightBottomPointX);
    rslt += GetFloatAttributeValue(pobjCollDetRegCfg, "CollDetReg", "RightBottomPoint_Y", stRadarDevCfg.stDetectParam.rightBottomPointY);
    rslt += GetFloatAttributeValue(pobjCollDetRegCfg, "CollDetReg", "LeftUpPoint_X",      stRadarDevCfg.stDetectParam.leftUpPointX);
    rslt += GetFloatAttributeValue(pobjCollDetRegCfg, "CollDetReg", "LeftUpPoint_Y",      stRadarDevCfg.stDetectParam.leftUpPointY);

    TiXmlElement *pobjInstallParamCfg = GetFirstChildElement(pobjCfgParam, "InstallParam");
    if (pobjInstallParamCfg == NULL)    return -1;
    rslt += GetFloatAttributeValue(pobjInstallParamCfg, "InstallParam", "Tx",   stRadarDevCfg.stInstallParam.tx);
    rslt += GetFloatAttributeValue(pobjInstallParamCfg, "InstallParam", "Ty",   stRadarDevCfg.stInstallParam.ty);
    rslt += GetFloatAttributeValue(pobjInstallParamCfg, "InstallParam", "Roll", stRadarDevCfg.stInstallParam.roll);
    rslt += GetFloatAttributeValue(pobjInstallParamCfg, "InstallParam", "Yaw",  stRadarDevCfg.stInstallParam.yaw);

//    ST_LOG_INFO("%s Param: isSupport:%d, Num:%u, RightBottomPoint_X:%f, RightBottomPoint_Y:%f, LeftUpPoint_X:%f, LeftUpPoint_Y:%f, Tx:%f, Ty:%f, Roll:%f, Yaw:%f",
//        element, stRadarDevCfg.isSupport, stRadarDevCfg.MaxObstCount, stRadarDevCfg.RightBottomPoint_X, stRadarDevCfg.RightBottomPoint_Y,
//        stRadarDevCfg.LeftUpPoint_X, stRadarDevCfg.LeftUpPoint_Y, stRadarDevCfg.stInstallParam.tx, stRadarDevCfg.stInstallParam.ty,
//        stRadarDevCfg.stInstallParam.roll, stRadarDevCfg.stInstallParam.yaw);
    return rslt ;
}

/**************************************************************************************
功能描述: 读取所有毫米波雷达配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadRadarCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_RADAR_STRU &stRadar)
{
    INT32 rslt = 0;
    
    TiXmlElement *pobjRadarGroupCfg = GetFirstChildElement(pobjCfgParam, "RadarGroup");
    if (pobjRadarGroupCfg == NULL)      return -1;
    rslt += GetBoolAttributeValue (pobjRadarGroupCfg, "RadarGroup", "isSupportCluster",      stRadar.stClusterParam.isSupportCluster);
    rslt += GetFloatAttributeValue(pobjRadarGroupCfg, "RadarGroup", "ClusterRadius",         stRadar.stClusterParam.clusterRadius);
    rslt += GetFloatAttributeValue(pobjRadarGroupCfg, "RadarGroup", "OutRadialVelThreshold", stRadar.stClusterParam.outRadialVelThreshold);

    TiXmlElement *pobjLeftRadarCfg = GetFirstChildElement(pobjRadarGroupCfg, "LeftRadar");
    if (pobjLeftRadarCfg == NULL)  return -1;
    rslt += ReadRadarDevCfg(pobjLeftRadarCfg, "LeftRadar", stRadar.stLeftRadar);

    TiXmlElement *pobjFrontRadarCfg = GetFirstChildElement(pobjRadarGroupCfg, "FrontRadar");
    if (pobjFrontRadarCfg == NULL) return -1;
    rslt += ReadRadarDevCfg(pobjFrontRadarCfg, "FrontRadar", stRadar.stFrontRadar);

    TiXmlElement *pobjRightRadarCfg = GetFirstChildElement(pobjRadarGroupCfg, "RightRadar");
    if (pobjRightRadarCfg == NULL) return -1;
    rslt += ReadRadarDevCfg(pobjRightRadarCfg, "RightRadar", stRadar.stRightRadar);

    TiXmlElement *pobjBackRadarCfg = GetFirstChildElement(pobjRadarGroupCfg, "BackRadar");
    if (pobjBackRadarCfg == NULL)  return -1;
    rslt += ReadRadarDevCfg(pobjBackRadarCfg, "BackRadar", stRadar.stBackRadar);

    if (rslt != 0) {
        ST_LOG_ERR("Read Radar Group Config Fail.");    
        return -1;
    }

    stRadar.stLeftRadar.stDetectParam.devID  = RADAR_LEFT_ID;
    stRadar.stFrontRadar.stDetectParam.devID = RADAR_FRONT_ID;
    stRadar.stRightRadar.stDetectParam.devID = RADAR_RIGHT_ID;
    stRadar.stBackRadar.stDetectParam.devID  = RADAR_BACK_ID;

//    ST_LOG_INFO("Radar General Param: isSupportCluster:%d ClusterRadius:%f OutRadialVelThreshold:%f OutVelModel:%d",
//        stRadar.isSupportCluster, stRadar.ClusterRadius, stRadar.OutRadialVelThreshold, stRadar.OutVelModel);
    
    return 0;
}

/**************************************************************************************
功能描述: 读取车体过滤空间信息
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadCarBodyVxlFilterCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_CAR_BODY_VXL_FILTER_STRU &stCarBodyVxlFilter)
{
    TiXmlElement *pobjCarBodyVxlFilterCfg = GetFirstChildElement(pobjCfgParam, "CarBodyVxlFilter");
    if (pobjCarBodyVxlFilterCfg == NULL)    return -1;
    
    INT32 rslt = GetBoolAttributeValue(pobjCarBodyVxlFilterCfg, "CarBodyVxlFilter", "isSupport", stCarBodyVxlFilter.isSupport);
    if (rslt != 0) return -1;

    if (!stCarBodyVxlFilter.isSupport) {
        ST_LOG_INFO("<%s> Cfg Param: <isSupport> = %d.", "CarBodyVxlFilter", stCarBodyVxlFilter.isSupport);
        return 0;
    }

    TiXmlElement *pobjCarBodyFilterSizeCfg = GetFirstChildElement(pobjCarBodyVxlFilterCfg, "FilterSize");
    if (pobjCarBodyFilterSizeCfg == NULL) return -1;
    rslt += GetFloatAttributeValue(pobjCarBodyFilterSizeCfg, "FilterSize", "VoxelSize", stCarBodyVxlFilter.stFilterSize.voxelSize);
    rslt += GetFloatAttributeValue(pobjCarBodyFilterSizeCfg, "FilterSize", "MinX",      stCarBodyVxlFilter.stFilterSize.stCuboid.minX);
    rslt += GetFloatAttributeValue(pobjCarBodyFilterSizeCfg, "FilterSize", "MaxX",      stCarBodyVxlFilter.stFilterSize.stCuboid.maxX);
    rslt += GetFloatAttributeValue(pobjCarBodyFilterSizeCfg, "FilterSize", "MinY",      stCarBodyVxlFilter.stFilterSize.stCuboid.minY);
    rslt += GetFloatAttributeValue(pobjCarBodyFilterSizeCfg, "FilterSize", "MaxY",      stCarBodyVxlFilter.stFilterSize.stCuboid.maxY);
    rslt += GetFloatAttributeValue(pobjCarBodyFilterSizeCfg, "FilterSize", "MinZ",      stCarBodyVxlFilter.stFilterSize.stCuboid.minZ);
    rslt += GetFloatAttributeValue(pobjCarBodyFilterSizeCfg, "FilterSize", "MaxZ",      stCarBodyVxlFilter.stFilterSize.stCuboid.maxZ);

    if (rslt != 0) {
        ST_LOG_ERR("Read Car Body Voxel Filter Config Fail.");
        return -1;
    }
    
    ST_LOG_INFO("Cfg Param Read Succ. <%s> Cfg Param: <isSupport> = %d.", "CarBodyVxlFilter", stCarBodyVxlFilter.isSupport);
    return 0;
}

/**************************************************************************************
功能描述: 读取激光雷达拖尾过滤配置参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadLidarShadowFilterCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_LIDAR_SHADOW_FILTER_STRU &stLidarShadowFilter)
{
    TiXmlElement *pobjLidarShadowFilter = GetFirstChildElement(pobjCfgParam, "LidarShadowFilter");
    if (pobjLidarShadowFilter == NULL)      return -1;

    INT32 rslt = GetBoolAttributeValue(pobjLidarShadowFilter, "LidarShadowFilter", "isSupport", stLidarShadowFilter.isSupport);
    if (rslt != 0)  return -1;
    
    ST_LOG_INFO("<%s> Cfg Param: <isSupport> = %d.", "LidarShadowFilter", stLidarShadowFilter.isSupport);

    return 0;
}

/**************************************************************************************
功能描述: 读取点云特征提取配置参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdFeatureExtrCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PCD_FEATURE_EXTR_STRU &stPcdFeatureExtr)
{
    TiXmlElement *pobjFeatureExtr = GetFirstChildElement(pobjCfgParam, "PcdFeatureExtr");
    if (pobjFeatureExtr == NULL)        return -1;

    TiXmlElement *pobjBreakFeature = GetFirstChildElement(pobjFeatureExtr, "BreakFeatureExtr");
    if (pobjBreakFeature == NULL)       return -1;

    TiXmlElement *pobjCornerFeature = GetFirstChildElement(pobjFeatureExtr, "CornerFeatureExtr");
    if (pobjCornerFeature == NULL)      return -1;

    INT32 rslt = 0;
    rslt += GetBoolAttributeValue(pobjBreakFeature,  "BreakFeatureExtr",  "isSupport", stPcdFeatureExtr.isSuptBreakFeat);
    rslt += GetBoolAttributeValue(pobjCornerFeature, "CornerFeatureExtr", "isSupport", stPcdFeatureExtr.isSuptCornerFeat);

    if (rslt != 0)  return -1;
    
    ST_LOG_INFO("<%s-%s> Cfg Param: <isSupport> = %d.", "PcdFeatureExtr", "BreakFeatureExtr",  stPcdFeatureExtr.isSuptBreakFeat);
    ST_LOG_INFO("<%s-%s> Cfg Param: <isSupport> = %d.", "PcdFeatureExtr", "CornerFeatureExtr", stPcdFeatureExtr.isSuptCornerFeat);

    return 0;
}

/**************************************************************************************
功能描述: 读取地面分割参数配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdGroundSegCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PCD_GROUND_SEG_STRU &stPcdGroundSeg)
{
    TiXmlElement *pobjPcdGroundSeg = GetFirstChildElement(pobjCfgParam, "PcdGroundSegmentation");
    if (pobjPcdGroundSeg == NULL)       return -1;

    TiXmlElement *pobjLidarGeneralParams = GetFirstChildElement(pobjPcdGroundSeg, "LidarGeneralParams");
    if (pobjLidarGeneralParams == NULL) return -1;

    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(pobjLidarGeneralParams, "LidarGeneralParams", "angleResolution", stPcdGroundSeg.resolution);
    rslt += GetIntAttributeValue(pobjLidarGeneralParams,   "LidarGeneralParams", "ScanPointsNum",   stPcdGroundSeg.scanPointsNum);

    TiXmlElement *pobjSegParams = GetFirstChildElement(pobjPcdGroundSeg, "SegParams");
    if (pobjSegParams == NULL)          return -1;
    rslt += GetFloatAttributeValue(pobjSegParams, "SegParams", "MaxLocalSlope",        stPcdGroundSeg.maxLocalSlope);
    rslt += GetFloatAttributeValue(pobjSegParams, "SegParams", "MaxGeneralSlope",      stPcdGroundSeg.maxGeneralSlope);
    rslt += GetFloatAttributeValue(pobjSegParams, "SegParams", "MinHeightThreshold",   stPcdGroundSeg.minHeightThreshold);
    rslt += GetFloatAttributeValue(pobjSegParams, "SegParams", "ReclassDistThreshold", stPcdGroundSeg.reclassDistThreshold);
    rslt += GetFloatAttributeValue(pobjSegParams, "SegParams", "RadiusThreshold",      stPcdGroundSeg.radiusThreshold);
    
    if(rslt != 0) return -1;
    ST_LOG_INFO("Cfg Param Read Succ.");
    return 0;
}

/**************************************************************************************
功能描述: 读取激光雷达点云过滤信息
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdFilterCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PCD_FILTER_STRU &stPcdFilter)
{
    TiXmlElement *pobjPcdFilterCfg = GetFirstChildElement(pobjCfgParam, "PcdFilter");
    if (pobjPcdFilterCfg == NULL)       return -1;

    TiXmlElement *pobjConditionFilterCfg = GetFirstChildElement(pobjPcdFilterCfg, "ConditionFilter");
    if (pobjConditionFilterCfg == NULL) return -1;
    INT32 rslt = GetBoolAttributeValue(pobjConditionFilterCfg, "ConditionFilter", "isSupport", stPcdFilter.stConditionFilter.isSupport);
    if (stPcdFilter.stConditionFilter.isSupport) {
        rslt += GetFloatAttributeValue(pobjConditionFilterCfg, "ConditionFilter", "Min_X", stPcdFilter.stConditionFilter.stCuboid.minX);
        rslt += GetFloatAttributeValue(pobjConditionFilterCfg, "ConditionFilter", "Max_X", stPcdFilter.stConditionFilter.stCuboid.maxX);
        rslt += GetFloatAttributeValue(pobjConditionFilterCfg, "ConditionFilter", "Min_Y", stPcdFilter.stConditionFilter.stCuboid.minY);
        rslt += GetFloatAttributeValue(pobjConditionFilterCfg, "ConditionFilter", "Max_Y", stPcdFilter.stConditionFilter.stCuboid.maxY);
        rslt += GetFloatAttributeValue(pobjConditionFilterCfg, "ConditionFilter", "Min_Z", stPcdFilter.stConditionFilter.stCuboid.minZ);
        rslt += GetFloatAttributeValue(pobjConditionFilterCfg, "ConditionFilter", "Max_Z", stPcdFilter.stConditionFilter.stCuboid.maxZ);
    }

    TiXmlElement *pobjVoxelGridFilterCfg = GetFirstChildElement(pobjPcdFilterCfg, "VoxelGridFilter");
    if (pobjVoxelGridFilterCfg == NULL) return -1;
    rslt = GetBoolAttributeValue(pobjVoxelGridFilterCfg, "VoxelGridFilter", "isSupport", stPcdFilter.stVoxelGridFilter.isSupport);
    if (stPcdFilter.stVoxelGridFilter.isSupport) {
        rslt +=  GetFloatAttributeValue(pobjVoxelGridFilterCfg, "VoxelGridFilter", "Size", stPcdFilter.stVoxelGridFilter.size);
    }
    
    TiXmlElement *pobjRadiusOutlierCfg = GetFirstChildElement(pobjPcdFilterCfg, "RadiusOutlierFilter");
    if (pobjRadiusOutlierCfg == NULL) return -1;
    rslt = GetBoolAttributeValue(pobjRadiusOutlierCfg, "RadiusOutlierFilter", "isSupport", stPcdFilter.stRadiusOutlier.isSupport);
    if (stPcdFilter.stRadiusOutlier.isSupport) {
        rslt += GetFloatAttributeValue(pobjRadiusOutlierCfg,    "RadiusOutlierFilter", "SearchRadius",         stPcdFilter.stRadiusOutlier.searchRadius);
        rslt += GetIntAttributeValue(pobjRadiusOutlierCfg, "RadiusOutlierFilter", "MinNeighborsInRadius", stPcdFilter.stRadiusOutlier.minNeighbors);
        if (stPcdFilter.stRadiusOutlier.minNeighbors < 0) {
            ST_LOG_ERR("Please Set the Correct Value of <%s>", "MinNeighborsInRadius");
            return -1;
        }
    }

    if (rslt != 0) {
        ST_LOG_ERR("Read Pcd Filter Config Fail.");
        return -1;
    }
    
    ST_LOG_INFO("<%s-%s> Cfg Param: <isSupport> = %d.", "PcdFilter", "ConditionFilter",     stPcdFilter.stConditionFilter.isSupport);
    ST_LOG_INFO("<%s-%s> Cfg Param: <isSupport> = %d.", "PcdFilter", "RadiusOutlierFilter", stPcdFilter.stRadiusOutlier.isSupport);
    ST_LOG_INFO("<%s-%s> Cfg Param: <isSupport> = %d.", "PcdFilter", "VoxelGridFilter",     stPcdFilter.stVoxelGridFilter.isSupport);
    return 0;
}

INT32 CFG_PARAM_FILE_C::ReadPcdHeightCfg(TiXmlElement *pobjCfgParam, const char *element, CFG_PARAM_PCD_HEIGHT_STRU &stPcdHeight)
{
    TiXmlElement *pobjPcdHeight = GetFirstChildElement(pobjCfgParam, element);
    if (pobjPcdHeight == NULL)  return -1;

    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(pobjPcdHeight, element, "min", stPcdHeight.min);
    rslt += GetFloatAttributeValue(pobjPcdHeight, element, "max", stPcdHeight.max);
    return rslt;
}

/**************************************************************************************
功能描述: 读取激光雷达点云按照地面高度划分信息
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdPartitionCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PCD_PARTITION_STRU &stPcdPartition)
{
    TiXmlElement *pobjPcdPartitionCfg = GetFirstChildElement(pobjCfgParam, "PcdPartition");
    if (pobjPcdPartitionCfg == NULL)       return -1;

    INT32 rslt = 0;
    rslt += ReadPcdHeightCfg(pobjPcdPartitionCfg, "UndergroundHeight", stPcdPartition.stUndergroundHeight);
    rslt += ReadPcdHeightCfg(pobjPcdPartitionCfg, "GeneralHeight",     stPcdPartition.stGeneralHeight);
    rslt += ReadPcdHeightCfg(pobjPcdPartitionCfg, "DangerHeight",      stPcdPartition.stDangerHeight);

    return rslt;
}

/**************************************************************************************
功能描述: 读取垃圾箱安装参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadDustbinInstallCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_INSTALL_POS_STRU &stInstallPos)
{
    TiXmlElement *pobjInstallPos = GetFirstChildElement(pobjCfgParam, "InstallParam");
    if (pobjInstallPos == NULL)  return -1;

    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(pobjInstallPos, "InstallParam", "Tx", stInstallPos.tx);
    rslt += GetFloatAttributeValue(pobjInstallPos, "InstallParam", "Ty", stInstallPos.ty);
    rslt += GetFloatAttributeValue(pobjInstallPos, "InstallParam", "Tz", stInstallPos.tz);

//    if (rslt == 0) {
//        ST_LOG_INFO("Dustbin Install Param: Tx=%f Ty=%f Tz=%f",
//            stInstallPos.tx, stInstallPos.ty, stInstallPos.tz);
//    }
    
    return rslt;
}

/**************************************************************************************
功能描述: 读取垃圾箱箱体参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadDustbinBoxCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_DUSTBIN_BOX_STRU &stDustbinBox)
{
    TiXmlElement *pobjDustbinBox = GetFirstChildElement(pobjCfgParam, "DustbinBox");
    if (pobjDustbinBox == NULL)  return -1;

    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(pobjDustbinBox, "DustbinBox", "LengthX", stDustbinBox.lengthX);
    rslt += GetFloatAttributeValue(pobjDustbinBox, "DustbinBox", "LengthY", stDustbinBox.lengthY);
    rslt += GetFloatAttributeValue(pobjDustbinBox, "DustbinBox", "LengthZ", stDustbinBox.lengthZ);

//    if (rslt == 0) {
//        ST_LOG_INFO("Dustbin Box Size: LengthX=%f LengthY=%f LengthZ=%f",
//            stDustbinBox.lengthX, stDustbinBox.lengthY, stDustbinBox.lengthZ);
//    }
    
    return rslt;
}

/**************************************************************************************
功能描述: 读取垃圾箱连杆参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadDustbinLinkCfg(TiXmlElement *pobjCfgParam, float &linkLength)
{
    TiXmlElement *pobjDustbinLink = GetFirstChildElement(pobjCfgParam, "DustbinLink");
    if (pobjDustbinLink == NULL)  return -1;

    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(pobjDustbinLink, "DustbinLink", "Length", linkLength);

//    if (rslt == 0) {
//         ST_LOG_INFO("Dustbin Link Length:%f", linkLength);
//    }
    
    return rslt;
}

/**************************************************************************************
功能描述: 读取垃圾箱检测区域
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadDustbinDetRegionCfg(TiXmlElement *pobjCfgParam, CUBOID_STRU &stDetRegion)
{
    TiXmlElement *pobjDetRegion = GetFirstChildElement(pobjCfgParam, "DetectRegion");
    if (pobjDetRegion == NULL)  return -1;

    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(pobjDetRegion, "DetectRegion", "MinX", stDetRegion.minX);
    rslt += GetFloatAttributeValue(pobjDetRegion, "DetectRegion", "MaxX", stDetRegion.maxX);
    rslt += GetFloatAttributeValue(pobjDetRegion, "DetectRegion", "MinY", stDetRegion.minY);
    rslt += GetFloatAttributeValue(pobjDetRegion, "DetectRegion", "MaxY", stDetRegion.maxY);
    rslt += GetFloatAttributeValue(pobjDetRegion, "DetectRegion", "MinZ", stDetRegion.minZ);
    rslt += GetFloatAttributeValue(pobjDetRegion, "DetectRegion", "MaxZ", stDetRegion.maxZ);

    if (rslt == 0) {
         ST_LOG_INFO("DetectRegion: MinX:%f MaxX:%f MinY:%f MaxY:%f MinZ:%f MaxZ:%f ",
            stDetRegion.minX, stDetRegion.maxX,
            stDetRegion.minY, stDetRegion.maxY,
            stDetRegion.minZ, stDetRegion.maxZ);
    }
    
    return rslt;
}

/**************************************************************************************
功能描述: 读取垃圾箱连杆参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdDustbinFilterCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PCD_DUSTBIN_FILTER_STRU &stPcdDustbinFilter)
{
    TiXmlElement *pobjDustbinFilter = GetFirstChildElement(pobjCfgParam, "DustbinFilter");
    if (pobjDustbinFilter == NULL)
        return -1;

    INT32 rslt = GetBoolAttributeValue(pobjDustbinFilter, "DustbinFilter", "isSupport", stPcdDustbinFilter.isSupport);    
    ST_LOG_INFO("%s Cfg Param: <isSupport> = %d.", "DustbinFilter", stPcdDustbinFilter.isSupport);
    
    if (!stPcdDustbinFilter.isSupport)
        return 0;

    rslt += ReadDustbinLinkCfg(pobjDustbinFilter, stPcdDustbinFilter.linkLength);
    rslt += ReadDustbinInstallCfg(pobjDustbinFilter, stPcdDustbinFilter.stInstallPos);
    rslt += ReadDustbinBoxCfg(pobjDustbinFilter, stPcdDustbinFilter.stDustbinBox);
    rslt += ReadDustbinDetRegionCfg(pobjDustbinFilter, stPcdDustbinFilter.stDetRegion);

    return rslt;
}

/**************************************************************************************
功能描述: 读取反光校验区验参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdReflectRegionCfg(TiXmlElement *pobjCfgParam, const char *element, RECTANGLE_STRU &stVerifyReg)
{
    TiXmlElement *pobjRegion = GetFirstChildElement(pobjCfgParam, "Region");
    if (pobjRegion == NULL) return -1;

    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(pobjRegion, "Region", "MinX", stVerifyReg.minX);
    rslt += GetFloatAttributeValue(pobjRegion, "Region", "MaxX", stVerifyReg.maxX);
    rslt += GetFloatAttributeValue(pobjRegion, "Region", "MinY", stVerifyReg.minY);
    rslt += GetFloatAttributeValue(pobjRegion, "Region", "MaxY", stVerifyReg.maxY);
//    ST_LOG_INFO("MinX:%f, MaxX:%f, MinY:%f, MaxY:%f", stVerifyReg.minX, stVerifyReg.maxX, stVerifyReg.minY, stVerifyReg.maxY);
    
    return rslt;
}

/**************************************************************************************
功能描述: 读取双激光雷达反光校验参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdDoubleLidarReflectVerifyCfg(TiXmlElement *pobjCfgParam, const char *element, CFG_PARAM_DOUBLE_LIDAR_VERIFY_STRU &stReflectVerify)
{
    TiXmlElement *pobjReflectVerify = GetFirstChildElement(pobjCfgParam, element);
    if (pobjReflectVerify == NULL) return -1;

    INT32 rslt = GetBoolAttributeValue(pobjReflectVerify, element, "isSupport", stReflectVerify.isSupport);
    ST_LOG_INFO("%s Cfg Param: <isSupport> = %d.", element, stReflectVerify.isSupport);
    if (!stReflectVerify.isSupport) return 0;

    rslt += ReadPcdReflectRegionCfg(pobjReflectVerify, "Region", stReflectVerify.stVerifyReg);
    
    return rslt;
}

/**************************************************************************************
功能描述: 读取突现反光校验参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdEmergenceReflectVerifyCfg(TiXmlElement *pobjCfgParam, const char *element, CFG_PARAM_EMERGENCE_VERIFY_STRU &stReflectVerify)
{
    TiXmlElement *pobjReflectVerify = GetFirstChildElement(pobjCfgParam, element);
    if (pobjReflectVerify == NULL) return -1;

    INT32 rslt = GetBoolAttributeValue(pobjReflectVerify, element, "isSupport", stReflectVerify.isSupport);
    ST_LOG_INFO("%s Cfg Param: <isSupport> = %d.", element, stReflectVerify.isSupport);
    if (!stReflectVerify.isSupport) return 0;

    rslt += ReadPcdReflectRegionCfg(pobjReflectVerify, "Region", stReflectVerify.stVerifyReg);

    TiXmlElement *pobjVerifyParam = GetFirstChildElement(pobjReflectVerify, "VerifyParam");
    if (pobjVerifyParam == NULL) return -1;
    rslt += GetFloatAttributeValue(pobjVerifyParam, "VerifyParam", "CompareRadius", stReflectVerify.CompareRadius);
    rslt += GetFloatAttributeValue(pobjVerifyParam, "VerifyParam", "KeyFrameDist",  stReflectVerify.keyFrameDist);
    
    return rslt;
}

/**************************************************************************************
功能描述: 读取雷达反光校验参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdReflectVerifyCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PCD_REFLECT_VERIFY_STRU &stPcdReflectVerify)
{
    TiXmlElement *pobjReflectVerify = GetFirstChildElement(pobjCfgParam, "ReflectVerify");
    if (pobjReflectVerify == NULL)          return -1;

    INT32 rslt = 0;
    rslt += ReadPcdDoubleLidarReflectVerifyCfg(pobjReflectVerify, "DoubleLidarVerify", stPcdReflectVerify.stDoubleLidarVerify);
    rslt += ReadPcdEmergenceReflectVerifyCfg(  pobjReflectVerify, "EmergenceVerify",   stPcdReflectVerify.stEmergenceVerify);

    return rslt;
}

/**************************************************************************************
功能描述: 读取斜坡路面点云处理参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdSlopeProcCfg(TiXmlElement *pobjCfgParam,CFG_PARAM_PCD_SLOPE_PROC_STRU &stPcdSlopeProc)
{
    TiXmlElement *pobjPcdSlopeProc = GetFirstChildElement(pobjCfgParam, "PcdSlopeProcess");
    if (pobjPcdSlopeProc == NULL)      return -1;

    INT32 rslt = GetBoolAttributeValue(pobjPcdSlopeProc, "PcdSlopeProcess", "isSupport", stPcdSlopeProc.isSupport);
    if (rslt != 0)  return -1;
    
    ST_LOG_INFO("<%s> Cfg Param: <isSupport> = %d.", "PcdSlopeProcess", stPcdSlopeProc.isSupport);

    return 0;
}

/**************************************************************************************
功能描述: 读取防跌落配置参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdFallArrestCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PCD_FALL_ARREST_STRU &stPcdFallArrest)
{
    TiXmlElement *pobjFallArrest = GetFirstChildElement(pobjCfgParam, "FallArrest");
    if (pobjFallArrest == NULL)      return -1;

    INT32 rslt = GetBoolAttributeValue(pobjFallArrest, "FallArrest", "isSupport", stPcdFallArrest.isSupport);
    if (rslt != 0)  return -1;
    
    ST_LOG_INFO("<%s> Cfg Param: <isSupport> = %d.", "FallArrest", stPcdFallArrest.isSupport);

    return 0;
}

/**************************************************************************************
功能描述: 读取低矮障碍物维持配置参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdShortObstHoldCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PCD_SHORT_OBST_HOLD_STRU &stPcdShortObstHold)
{
    TiXmlElement *pobjShortObsrHold = GetFirstChildElement(pobjCfgParam, "ShortObstHold");
    if (pobjShortObsrHold == NULL)      return -1;

    INT32 rslt = GetBoolAttributeValue(pobjShortObsrHold, "ShortObstHold", "isSupport", stPcdShortObstHold.isSupport);
    if (rslt != 0)  return -1;
    
    ST_LOG_INFO("<%s> Cfg Param: <isSupport> = %d.", "ShortObstHold", stPcdShortObstHold.isSupport);

    return 0;
}

/**************************************************************************************
功能描述: 读取激光雷达拖尾过滤配置参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdImpassAreaDetCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PCD_IMPASS_AREA_DET_STRU &stPcdImpassAreaDet)
{
    TiXmlElement *pobjImpassAreaDet = GetFirstChildElement(pobjCfgParam, "ImpassableAreaDet");
    if (pobjImpassAreaDet == NULL)      return -1;

    INT32 rslt = GetBoolAttributeValue(pobjImpassAreaDet, "ImpassableAreaDet", "isSupport", stPcdImpassAreaDet.isSupport);
    if (rslt != 0)  return -1;
    
    ST_LOG_INFO("<%s> Cfg Param: <isSupport> = %d.", "ImpassableAreaDet", stPcdImpassAreaDet.isSupport);

    return 0;
}

/**************************************************************************************
功能描述: 读取点云聚类区域配置参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdRegionCfg(TiXmlElement *pobjCfgParam, const char *element, CFG_PARAM_REGION_STRU &stRegion)
{
    INT32 rslt = 0;
    rslt += GetFloatAttributeValue(   pobjCfgParam, "RegCfg", "OriginDistance",  stRegion.originDistance);
    rslt += GetFloatAttributeValue(   pobjCfgParam, "RegCfg", "ClusterRadius",   stRegion.clusterRadius);
    rslt += GetUnsignedAttributeValue(pobjCfgParam, "RegCfg", "MinCluPointsNum", stRegion.minCluPointsNum);
    rslt += GetUnsignedAttributeValue(pobjCfgParam, "RegCfg", "MaxCluPointsNum", stRegion.maxCluPointsNum);
    if (stRegion.minCluPointsNum > stRegion.maxCluPointsNum) {
        ST_LOG_ERR("Points Num <%u~%u> Error. Please Set the Correct Points Num. Note: MinCluPointsNum <= MaxCluPointsNum", stRegion.minCluPointsNum, stRegion.maxCluPointsNum);
        return -1;
    }

//    ST_LOG_INFO("Pcd Region Partition Param:%f %f %u %u", stRegion.originDistance, stRegion.clusterRadius, stRegion.minCluPointsNum, stRegion.maxCluPointsNum);
    return rslt;
}

/**************************************************************************************
功能描述: 读取点云聚类类别配置参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdClusterClassCfg(TiXmlElement *pobjCfgParam, const char *element, CFG_PARAM_PCD_CLUSTER_CLASS_STRU &stPcdClusterClass)
{
    TiXmlElement *pobjPcdClusterClassCfg = GetFirstChildElement(pobjCfgParam, element);
    if(pobjPcdClusterClassCfg == NULL) return -1;

    INT32 rslt = GetBoolAttributeValue(pobjPcdClusterClassCfg, element, "isSupport", stPcdClusterClass.isSupport);
    if (rslt == 0 && stPcdClusterClass.isSupport) {
        rslt += GetFloatAttributeValue(pobjPcdClusterClassCfg, element, "MinHeight", stPcdClusterClass.minHeight);
        rslt += GetFloatAttributeValue(pobjPcdClusterClassCfg, element, "MinLength", stPcdClusterClass.minLength);
        rslt += GetFloatAttributeValue(pobjPcdClusterClassCfg, element, "MaxLength", stPcdClusterClass.maxLength);
    }
    
    return rslt;
}

/**************************************************************************************
功能描述: 读取点云聚类参数配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdClusterCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PCD_CLUSTER_STRU &stPcdCluster)
{
    TiXmlElement *pobjPcdClusterCfg = GetFirstChildElement(pobjCfgParam, "PcdCluster");
    if (pobjPcdClusterCfg == NULL)      return -1;

    INT32 rslt = GetBoolAttributeValue(pobjPcdClusterCfg, "PcdCluster", "isSupport", stPcdCluster.isSupport);
    if (rslt != 0)  return -1;

    ST_LOG_INFO("<%s> Cfg Param: <isSupport> = %d.", "PcdCluster", stPcdCluster.isSupport);
    
    if (!stPcdCluster.isSupport) return 0;

    TiXmlElement *pobjVoxelGridCfg = GetFirstChildElement(pobjPcdClusterCfg, "VoxelGrid");
    if (pobjVoxelGridCfg == NULL)       return -1;
    rslt += GetFloatAttributeValue(pobjVoxelGridCfg, "VoxelGrid", "Size", stPcdCluster.stVoxelGrid.size);

    TiXmlElement *pobjRegionPartitionCfg = GetFirstChildElement(pobjPcdClusterCfg, "RegionPartition");
    if (pobjRegionPartitionCfg == NULL) return -1;
    CFG_PARAM_REGION_STRU stRegion;
    TiXmlElement *pobjRegionCfg = GetFirstChildElement(pobjRegionPartitionCfg, "RegCfg");
    if (pobjRegionCfg == NULL)          return -1;
    for(; pobjRegionCfg != NULL; pobjRegionCfg = pobjRegionCfg->NextSiblingElement()) {
        rslt += ReadPcdRegionCfg(pobjRegionCfg, "RegCfg", stRegion);
        stPcdCluster.vstRegion.push_back(stRegion);
    }

    rslt += ReadPcdClusterClassCfg(pobjPcdClusterCfg, "DetectTruck", stPcdCluster.stTruckDetect);
    rslt += ReadPcdClusterClassCfg(pobjPcdClusterCfg, "DetectCar",   stPcdCluster.stCarDetect);
    rslt += ReadPcdClusterClassCfg(pobjPcdClusterCfg, "DetectMan",   stPcdCluster.stManDetect);

    TiXmlElement *pobjSpeedMeasureCfg = GetFirstChildElement(pobjPcdClusterCfg, "SpeedMeasure");
    if(pobjSpeedMeasureCfg == NULL)   return -1;

    TiXmlElement *pobjSpeedMeasureLimitCfg = GetFirstChildElement(pobjSpeedMeasureCfg, "LinearSpeedLimit");
    if(pobjSpeedMeasureLimitCfg == NULL)   return -1;
    rslt += GetBoolAttributeValue( pobjSpeedMeasureLimitCfg, "LinearSpeedLimit",     "isSupport",         stPcdCluster.stPcdClusterSpeed.stLinearSpeedLimit.isSupport);
    rslt += GetFloatAttributeValue(pobjSpeedMeasureLimitCfg, "LinearSpeedLimit",     "MaxLinearSpeed",    stPcdCluster.stPcdClusterSpeed.stLinearSpeedLimit.maxLinearSpeed);

    pobjSpeedMeasureLimitCfg = GetFirstChildElement(pobjSpeedMeasureCfg, "AccSpeedLimit");
    if(pobjSpeedMeasureLimitCfg == NULL)   return -1;
    rslt += GetBoolAttributeValue( pobjSpeedMeasureLimitCfg, "AccSpeedLimit",        "isSupport",         stPcdCluster.stPcdClusterSpeed.stLinearAccSpeedLimit.isSupport);
    rslt += GetFloatAttributeValue(pobjSpeedMeasureLimitCfg, "AccSpeedLimit",        "MaxLinearAccSpeed", stPcdCluster.stPcdClusterSpeed.stLinearAccSpeedLimit.maxLinearAccSpeed);

    pobjSpeedMeasureLimitCfg = GetFirstChildElement(pobjSpeedMeasureCfg, "AngularSpeedLimit");
    if(pobjSpeedMeasureLimitCfg == NULL)   return -1;
    rslt += GetBoolAttributeValue( pobjSpeedMeasureLimitCfg, "AngularSpeedLimit",    "isSupport",         stPcdCluster.stPcdClusterSpeed.stAngularSpeedLimit.isSupport);
    rslt += GetFloatAttributeValue(pobjSpeedMeasureLimitCfg, "AngularSpeedLimit",    "MaxAngularSpeed",   stPcdCluster.stPcdClusterSpeed.stAngularSpeedLimit.maxAngularSpeed);
    
    pobjSpeedMeasureLimitCfg = GetFirstChildElement(pobjSpeedMeasureCfg, "BoxBulkBiasRateLimit");
    if(pobjSpeedMeasureLimitCfg == NULL)   return -1;
    rslt += GetBoolAttributeValue( pobjSpeedMeasureLimitCfg, "BoxBulkBiasRateLimit", "isSupport",         stPcdCluster.stPcdClusterSpeed.stBoxBulkBiasRateLimit.isSupport);
    rslt += GetFloatAttributeValue(pobjSpeedMeasureLimitCfg, "BoxBulkBiasRateLimit", "BoxBulkBiasRate",   stPcdCluster.stPcdClusterSpeed.stBoxBulkBiasRateLimit.boxBulkBiasRate);

    return rslt;
}

/**************************************************************************************
功能描述: 读取点云数据处理相关参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPcdCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PCD_STRU &stPcdParam)
{
    TiXmlElement *pobjPcdCfg = GetFirstChildElement(pobjCfgParam, "Pcd");
    if (pobjPcdCfg == NULL){
        ST_LOG_INFO("Cfg Param Don't Has the Element <%s>.", "Pcd");
        return -1;
    }

    int rslt = 0;
    rslt += ReadCarBodyVxlFilterCfg( pobjPcdCfg, stPcdParam.stCarBodyVxlFilter);
    rslt += ReadLidarShadowFilterCfg(pobjPcdCfg, stPcdParam.stLidarShadowFilter);
    rslt += ReadPcdFeatureExtrCfg(   pobjPcdCfg, stPcdParam.stPcdFeatureExtr);
    rslt += ReadPcdGroundSegCfg(     pobjPcdCfg, stPcdParam.stPcdGroundSeg);
    rslt += ReadPcdFilterCfg(        pobjPcdCfg, stPcdParam.stPcdFilter);
    rslt += ReadPcdPartitionCfg(     pobjPcdCfg, stPcdParam.stPcdPartition);
    rslt += ReadPcdDustbinFilterCfg( pobjPcdCfg, stPcdParam.stPcdDustbinFilter);
    rslt += ReadPcdReflectVerifyCfg( pobjPcdCfg, stPcdParam.stPcdReflectVerify);
    rslt += ReadPcdSlopeProcCfg(     pobjPcdCfg, stPcdParam.stPcdSlopeProc);
    rslt += ReadPcdFallArrestCfg(    pobjPcdCfg, stPcdParam.stPcdFallArrest);
    rslt += ReadPcdShortObstHoldCfg( pobjPcdCfg, stPcdParam.stPcdShortObstHold);
    rslt += ReadPcdImpassAreaDetCfg( pobjPcdCfg, stPcdParam.stPcdImpassAreaDet);
    rslt += ReadPcdClusterCfg(       pobjPcdCfg, stPcdParam.stPcdCluster);
    return rslt;
}

/**************************************************************************************
功能描述: 读取底盘轮速计参数配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadWheelSpeedSensorCfg(TiXmlElement *pobjVehicleChassisCfgParam, CFG_PARAM_WHEEL_SPEED_SENSOR_STRU &stWheelSpeedSensorCfg) const
{
    TiXmlElement *pobjWheelSpeedSensorCfg = pobjVehicleChassisCfgParam->FirstChildElement("WheelSpeedSensor");
    if (pobjWheelSpeedSensorCfg == NULL) {
        ST_LOG_ERR("Get <WheelSpeedSensor> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjWheelSpeedSensorCfg->QueryFloatAttribute("ScaleFactor", &stWheelSpeedSensorCfg.scaleFactor)) {
        ST_LOG_ERR("Get <WheelSpeedSensor> Cfg Attr <ScaleFactor> Fail.");
        return -1;
    }

    ST_LOG_INFO("<WheelSpeedSensor> Cfg: <ScaleFactor> = %f.", stWheelSpeedSensorCfg.scaleFactor);
    return 0;
}

/**************************************************************************************
功能描述: 读取转向角传感器参数配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadSteeringAngleSensorCfg(TiXmlElement *pobjVehicleChassisCfgParam, CFG_PARAM_STEERING_ANGLE_SENSOR_STRU &stSteeringAngleSensorCfg) const
{
    TiXmlElement *pobjSteeringAngleSensorCfg = pobjVehicleChassisCfgParam->FirstChildElement("SteeringAngleSensor");
    if (pobjSteeringAngleSensorCfg == NULL) {
        ST_LOG_ERR("Get <SteeringAngleSensor> Cfg Element Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjSteeringAngleSensorCfg->QueryFloatAttribute("ScaleFactor", &stSteeringAngleSensorCfg.scaleFactor)) {
        ST_LOG_ERR("Get <SteeringAngleSensor> Cfg Attr <ScaleFactor> Fail.");
        return -1;
    }

    if (TIXML_SUCCESS != pobjSteeringAngleSensorCfg->QueryFloatAttribute("OffsetFactor", &stSteeringAngleSensorCfg.offsetFactor)) {
        ST_LOG_ERR("Get <SteeringAngleSensor> Cfg Attr <OffsetFactor> Fail.");
        return -1;
    }

    ST_LOG_INFO("<SteeringAngleSensor> Cfg: <ScaleFactor> = %f, <OffsetFactor> = %f.", stSteeringAngleSensorCfg.scaleFactor, stSteeringAngleSensorCfg.offsetFactor);
    return 0;
}

/**************************************************************************************
功能描述: 读取速度平滑器参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadVelSmootherCfg(TiXmlElement *pobjVehicleChassisCfgParam, CFG_PARAM_VEL_SMOOTHER_STRU &stVelSmootherCfg) const
{
    TiXmlElement *pobjVelSmootherCfg = pobjVehicleChassisCfgParam->FirstChildElement("VelSmoother");
    if (pobjVelSmootherCfg == NULL) {
        ST_LOG_ERR("Get <VelSmoother> Cfg Element Fail.");
        return -1;
    }

#define READ_VEL_SMOOTHER_CFG_ATTR(attrName_, attrVal_)                                             \
    if (TIXML_SUCCESS != pobjVelSmootherCfg->QueryFloatAttribute(attrName_, &attrVal_)) {           \
        ST_LOG_ERR("Get <VelSmoother> Cfg Attr <" attrName_ "> Fail.");                             \
        return -1;                                                                                  \
    }

    READ_VEL_SMOOTHER_CFG_ATTR("forwardAcc",    stVelSmootherCfg.forwardAcc);
    READ_VEL_SMOOTHER_CFG_ATTR("forwardDec",    stVelSmootherCfg.forwardDec);
    READ_VEL_SMOOTHER_CFG_ATTR("backwardAcc",   stVelSmootherCfg.backwardAcc);
    READ_VEL_SMOOTHER_CFG_ATTR("backwardDec",   stVelSmootherCfg.backwardDec);

#undef READ_VEL_SMOOTHER_CFG_ATTR

    ST_LOG_INFO("<VelSmoother> Cfg: <forwardAcc> = %f, <forwardDec> = %f, <backwardAcc> = %f, <backwardDec> = %f.", 
                stVelSmootherCfg.forwardAcc,
                stVelSmootherCfg.forwardDec,
                stVelSmootherCfg.backwardAcc,
                stVelSmootherCfg.backwardDec);
    return 0;
}

/**************************************************************************************
功能描述: 读取底盘参数配置
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadVehicleChassisCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_VEHICLE_CHASSIS_STRU &stVehicleChssisCfg) const
{
    TiXmlElement *pobjVehicleChassisCfg = pobjCfgParam->FirstChildElement("VehicleChassis");
    if (pobjVehicleChassisCfg == NULL) {
        ST_LOG_ERR("Get <VehicleChassis> Cfg Element Fail.");
        return -1;
    }

#define READ_VEHICLE_CHASSIS_ATTR(pfRead_, attrVal_)                    \
    if (0 != pfRead_(pobjVehicleChassisCfg, attrVal_)) {                \
        ST_LOG_ERR(#pfRead_"() Fail.");                                 \
        return -1;                                                      \
    }

    READ_VEHICLE_CHASSIS_ATTR(ReadWheelSpeedSensorCfg,      stVehicleChssisCfg.stWheelSpeedSensor);
    READ_VEHICLE_CHASSIS_ATTR(ReadSteeringAngleSensorCfg,   stVehicleChssisCfg.stSteeringAngleSensor);
    READ_VEHICLE_CHASSIS_ATTR(ReadVelSmootherCfg,           stVehicleChssisCfg.stVelSmoother);

#undef READ_VEHICLE_CHASSIS_ATTR
    
    ST_LOG_INFO("Read <VehicleChassis> Succ.");
    return 0;
}

/**************************************************************************************
功能描述: 读取线速度规划参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadLinearVelPlanCfg(TiXmlElement *pobjPlanCfgParam, CFG_PARAM_LINEAR_VEL_PLAN_STRU &stLinearVelPlanCfg) const
{
    TiXmlElement *pobjLinearVelPlanCfg = pobjPlanCfgParam->FirstChildElement("LinearVelPlan");
    if (pobjLinearVelPlanCfg == NULL) {
        ST_LOG_ERR("Get <LinearVelPlan> Cfg Element Fail.");
        return -1;
    }

#define READ_LINEAR_VEL_PLAN_CFG_ATTR(attrName_, attrVal_)                                          \
    if (TIXML_SUCCESS != pobjLinearVelPlanCfg->QueryFloatAttribute(attrName_, &attrVal_)) {         \
        ST_LOG_ERR("Get <LinearVelPlan> Cfg Attr <" attrName_ "> Fail.");                           \
        return -1;                                                                                  \
    }

    READ_LINEAR_VEL_PLAN_CFG_ATTR("MaxAngularVel",      stLinearVelPlanCfg.maxAngularVel);
    READ_LINEAR_VEL_PLAN_CFG_ATTR("SmoothDec",          stLinearVelPlanCfg.smoothDec);
    READ_LINEAR_VEL_PLAN_CFG_ATTR("VelCtrlDelayFactor", stLinearVelPlanCfg.velCtrlDelayFactor);

#undef READ_LINEAR_VEL_PLAN_CFG_ATTR

    ST_LOG_INFO("<LinearVelPlan> Cfg: <MaxAngularVel> = %f, <SmoothDec> = %f, <VelCtrlDelayFactor> = %f.", 
                stLinearVelPlanCfg.maxAngularVel,
                stLinearVelPlanCfg.smoothDec,
                stLinearVelPlanCfg.velCtrlDelayFactor);
    return 0;
}

/**************************************************************************************
功能描述: 读取规划配置参数
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_FILE_C::ReadPlanCfg(TiXmlElement *pobjCfgParam, CFG_PARAM_PLAN_STRU &stPlanCfg) const
{
    TiXmlElement *pobjPlanCfg = pobjCfgParam->FirstChildElement("Plan");
    if (pobjPlanCfg == NULL) {
        ST_LOG_ERR("Get <Plan> Cfg Element Fail.");
        return -1;
    }

#define READ_PLAN_ATTR(pfRead_, attrVal_)                               \
    if (0 != pfRead_(pobjPlanCfg, attrVal_)) {                          \
        ST_LOG_ERR(#pfRead_"() Fail.");                                 \
        return -1;                                                      \
    }

    READ_PLAN_ATTR(ReadLinearVelPlanCfg, stPlanCfg.stLinearVelPlan);

#undef READ_PLAN_ATTR

    ST_LOG_INFO("Read <Plan> Succ.");
    return 0;
}

/**************************************************************************************
功能描述: 静态变量定义
修改记录:
**************************************************************************************/
BOOL           CFG_PARAM_C::isInitSucc_ = false;
CFG_PARAM_STRU CFG_PARAM_C::stCfgParam_ = {{0}};

/**************************************************************************************
功能描述: 初始化
          多线程时如何处理? 初始化中读文件本身有锁，此处暂处理
修改记录:
**************************************************************************************/
INT32 CFG_PARAM_C::Init(void)
{
    if (isInitSucc_) {
        ST_LOG_INFO("Inited Before.");
        return 0;
    }
    
    CFG_PARAM_FILE_C objCfgParamFile;
    INT32 rslt = objCfgParamFile.Read(stCfgParam_);
    if (rslt != 0) {
        ST_LOG_ERR("objCfgParamFile.Read() Fail.");
        return -1;
    }

    isInitSucc_ = true;
    ST_LOG_INFO("Succ.");
    return 0;
}

BOOL CFG_PARAM_C::IsSupportGps(void)
{
    return stCfgParam_.stGps.isSupport;
}

BOOL CFG_PARAM_C::IsGpsDualAntenna(void)
{
    return stCfgParam_.stGps.isDualAntenna;
}

BOOL CFG_PARAM_C::IsSupportImu(void)
{
    return stCfgParam_.stImu.isSupport;
}

BOOL CFG_PARAM_C::IsSupportLocLidar(void)
{
    return stCfgParam_.stLocLidar.isSupport;
}

BOOL CFG_PARAM_C::IsSupportHeadDepthCamera(void)
{
    return stCfgParam_.stHeadDepthCamera.isSupport;
}

BOOL CFG_PARAM_C::IsSupportBackDepthCamera(void)
{
    return stCfgParam_.stBackDepthCamera.isSupport;
}

BOOL CFG_PARAM_C::IsSupportLeftBackDepthCamera(void)
{
    return stCfgParam_.stLeftBackDepthCamera.isSupport;
}

BOOL CFG_PARAM_C::IsSupportRightBackDepthCamera(void)
{
    return stCfgParam_.stRightBackDepthCamera.isSupport;
}

BOOL CFG_PARAM_C::IsSupportVisionDetectMan(void)
{
    if (stCfgParam_.stVisionDetect.stHeadWideAngleCam.stDetPedCar.isSupport || 
        stCfgParam_.stVisionDetect.stBackWideAngleCam.stDetPedCar.isSupport) {
        return true;
    } else {
        return false;
    }
}

BOOL CFG_PARAM_C::IsSupportVisionDetectCar(void)
{
    return IsSupportVisionDetectMan();
}

BOOL CFG_PARAM_C::IsSupportVisionDetectLane(void)
{
    if (stCfgParam_.stVisionDetect.stHeadWideAngleCam.stDetLane.isSupport || 
        stCfgParam_.stVisionDetect.stBackWideAngleCam.stDetPedCar.isSupport) {
        return true;
    } else {
        return false;
    }
}

BOOL CFG_PARAM_C::IsSupportVisionDetectTrafficLight(void)
{
    return stCfgParam_.stVisionDetect.stHeadLongFocusCam.stDetTrafficLight.isSupport;
}

BOOL CFG_PARAM_C::IsSupportHeadLeftRs16Lidar(void)
{
    return stCfgParam_.stLidar.stHeadLeftRs16Lidar.isSupport;
}

BOOL CFG_PARAM_C::IsSupportHeadLeftRsBpearlLidar(void)
{
    return stCfgParam_.stLidar.stHeadLeftRsBpearlLidar.isSupport;
}

BOOL CFG_PARAM_C::IsSupportHeadRightRsBpearlLidar(void)
{
    return stCfgParam_.stLidar.stHeadRightRsBpearlLidar.isSupport;
}

BOOL CFG_PARAM_C::IsSupportMiddleRs16Lidar(void)
{
    return stCfgParam_.stLidar.stMiddleRs16Lidar.isSupport;
}

BOOL CFG_PARAM_C::IsSupportMiddleRsBpearlLidar(void)
{
    return stCfgParam_.stLidar.stMiddleRsBpearlLidar.isSupport;
}

CFG_PARAM_HEAD_LIDAR_STRU CFG_PARAM_C::GetHeadLeftRs16LidarParam(void)
{
    return stCfgParam_.stLidar.stHeadLeftRs16Lidar;
}

CFG_PARAM_HEAD_LIDAR_STRU CFG_PARAM_C::GetHeadLeftRsBpearlLidarParam(void)
{
    return stCfgParam_.stLidar.stHeadLeftRsBpearlLidar;
}

CFG_PARAM_HEAD_LIDAR_STRU CFG_PARAM_C::GetHeadRightRsBpearlLidarParam(void)
{
    return stCfgParam_.stLidar.stHeadRightRsBpearlLidar;
}

CFG_PARAM_MIDDLE_LIDAR_STRU CFG_PARAM_C::GetMiddleRs16LidarParam(void)
{
    return stCfgParam_.stLidar.stMiddleRs16Lidar;
}

CFG_PARAM_MIDDLE_LIDAR_STRU CFG_PARAM_C::GetMiddleRsBpearlLidarParam(void)
{
    return stCfgParam_.stLidar.stMiddleRsBpearlLidar;
}

BOOL CFG_PARAM_C::IsUseHeadLeftRs16Lidar(void)
{
    return stCfgParam_.stMultiLidarFusion.isUseHeadLeftRs16Lidar;
}

BOOL CFG_PARAM_C::IsUseHeadLeftRsBpearlLidar(void)
{
    return stCfgParam_.stMultiLidarFusion.isUseHeadLeftRsBpearlLidar;
}

BOOL CFG_PARAM_C::IsUseHeadRightRsBpearlLidar(void)
{
    return stCfgParam_.stMultiLidarFusion.isUseHeadRightRsBpearlLidar;
}

BOOL CFG_PARAM_C::IsUseMiddleRs16Lidar(void)
{
    return stCfgParam_.stMultiLidarFusion.isUseMiddleRs16Lidar;
}

BOOL CFG_PARAM_C::IsUseMiddleRsBpearlLidar(void)
{
    return stCfgParam_.stMultiLidarFusion.isUseMiddleRsBpearlLidar;
}

BOOL CFG_PARAM_C::IsUndistort(void)
{
    return stCfgParam_.stMultiLidarFusion.isUndistort;
}

BOOL CFG_PARAM_C::IsSupportLeftRadar(void)
{
    return stCfgParam_.stRadar.stLeftRadar.isSupport;
}

BOOL CFG_PARAM_C::IsSupportFrontRadar(void)
{
    return stCfgParam_.stRadar.stFrontRadar.isSupport;
}

BOOL CFG_PARAM_C::IsSupportRightRadar(void)
{
    return stCfgParam_.stRadar.stRightRadar.isSupport;
}

BOOL CFG_PARAM_C::IsSupportBackRadar(void)
{
    return stCfgParam_.stRadar.stBackRadar.isSupport;
}

BOOL CFG_PARAM_C::IsSupportCarBodyVxlFilter(void)
{
    return stCfgParam_.stPcd.stCarBodyVxlFilter.isSupport;
}

BOOL CFG_PARAM_C::IsSupportLidarShadowFilter(void)
{
    return stCfgParam_.stPcd.stLidarShadowFilter.isSupport;
}

BOOL CFG_PARAM_C::IsSupportPcdBreakFeatureExtr(void)
{
    return stCfgParam_.stPcd.stPcdFeatureExtr.isSuptBreakFeat;
}

BOOL CFG_PARAM_C::IsSupportPcdCornerFeatureExtr(void)
{
    return stCfgParam_.stPcd.stPcdFeatureExtr.isSuptCornerFeat;
}

BOOL CFG_PARAM_C::IsSupportPcdDustbinFilter(void)
{
    return stCfgParam_.stPcd.stPcdDustbinFilter.isSupport;
}

BOOL CFG_PARAM_C::IsSupportPcdConditionFilter(void)
{
    return stCfgParam_.stPcd.stPcdFilter.stConditionFilter.isSupport;
}

BOOL CFG_PARAM_C::IsSupportPcdVoexlGridFilter(void)
{
    return stCfgParam_.stPcd.stPcdFilter.stVoxelGridFilter.isSupport;
}

BOOL CFG_PARAM_C::IsSupportPcdRadiusOutlierFilter(void)
{
    return stCfgParam_.stPcd.stPcdFilter.stRadiusOutlier.isSupport;
}

BOOL CFG_PARAM_C::IsSupportDoubleLidarReflectVerify(void)
{
    return stCfgParam_.stPcd.stPcdReflectVerify.stDoubleLidarVerify.isSupport;
}

BOOL CFG_PARAM_C::IsSupportEmergenceReflectVerify(void)
{
    return stCfgParam_.stPcd.stPcdReflectVerify.stEmergenceVerify.isSupport;
}

BOOL CFG_PARAM_C::IsSupportPcdSlopeProc(void)
{
    return stCfgParam_.stPcd.stPcdSlopeProc.isSupport;
}

BOOL CFG_PARAM_C::IsSupportPcdFallArrest(void)
{
    return stCfgParam_.stPcd.stPcdFallArrest.isSupport;
}

BOOL CFG_PARAM_C::IsSupportPcdShortObstHold(void)
{
    return stCfgParam_.stPcd.stPcdShortObstHold.isSupport;
}

BOOL CFG_PARAM_C::IsSupportPcdImpassAreaDet(void)
{
    return stCfgParam_.stPcd.stPcdImpassAreaDet.isSupport;
}

BOOL CFG_PARAM_C::IsSupportPcdCluster(void)
{
    return stCfgParam_.stPcd.stPcdCluster.isSupport;
}

string CFG_PARAM_C::GetGpsDevName(void) const
{
    return stCfgParam_.stGps.strDevName;
}
INT32  CFG_PARAM_C::GetGpsBaudRate(void) const
{
    return stCfgParam_.stGps.baudRate;
}
string CFG_PARAM_C::GetImuDevName(void) const
{
    return stCfgParam_.stImu.strDevName;
}

INT32  CFG_PARAM_C::GetImuBaudRate(void) const
{
    return stCfgParam_.stImu.baudRate;
}

BOOL   CFG_PARAM_C::IsModifyImuGyroZ(void) const
{
    return stCfgParam_.stImu.isModifyGyroZ;
}

CFG_PARAM_INSTALL_STRU &CFG_PARAM_C::GetGpsInstallPose(void) const
{
    return stCfgParam_.stGps.stInstallPose;
}

CFG_PARAM_INSTALL_STRU &CFG_PARAM_C::GetImuInstallPose(void) const
{
    return stCfgParam_.stImu.stInstallPose;
}

CFG_PARAM_INSTALL_STRU &CFG_PARAM_C::GetLocLidarInstallPose(void) const
{
    return stCfgParam_.stLocLidar.stInstallPose;
}

CFG_PARAM_INSTALL_POS_STRU &CFG_PARAM_C::GetHeadDepthCameraInstallCfg(void) const
{
    return stCfgParam_.stHeadDepthCamera.stInstallPos;
}

CFG_PARAM_INSTALL_POS_STRU &CFG_PARAM_C::GetBackDepthCameraInstallCfg(void) const
{
    return stCfgParam_.stBackDepthCamera.stInstallPos;
}

CFG_PARAM_INSTALL_STRU &CFG_PARAM_C::GetLeftBackDepthCameraInstallCfg(void) const
{
    return stCfgParam_.stLeftBackDepthCamera.stInstallPose;
}

CFG_PARAM_INSTALL_STRU &CFG_PARAM_C::GetRightBackDepthCameraInstallCfg(void) const
{
    return stCfgParam_.stRightBackDepthCamera.stInstallPose;
}

CFG_PARAM_PLANE_FIT_STRU &CFG_PARAM_C::GetHeadDepthCameraPlaneFitCfg(void) const
{
    return stCfgParam_.stHeadDepthCamera.stPlaneFitParam;
}

CFG_PARAM_PLANE_FIT_STRU &CFG_PARAM_C::GetBackDepthCameraPlaneFitCfg(void) const
{
    return stCfgParam_.stBackDepthCamera.stPlaneFitParam;
}

CFG_PARAM_RADAR_STRU &CFG_PARAM_C::GetRadarCfg(void) const
{
    return stCfgParam_.stRadar;
}

CFG_PARAM_CAR_BODY_VXL_FILTER_STRU &CFG_PARAM_C::GetCarBodyVxlFilterCfg(void) const
{
    return stCfgParam_.stPcd.stCarBodyVxlFilter;
}

CFG_PARAM_LIDAR_SHADOW_FILTER_STRU &CFG_PARAM_C::GetLidarShadowFilter(void) const
{
    return stCfgParam_.stPcd.stLidarShadowFilter;
}

CFG_PARAM_PCD_FEATURE_EXTR_STRU &CFG_PARAM_C::GetPcdFeatureExtrCfg(void) const
{
    return stCfgParam_.stPcd.stPcdFeatureExtr;
}

CFG_PARAM_PCD_GROUND_SEG_STRU &CFG_PARAM_C::GetPcdGroundSegCfg(void) const
{
    return stCfgParam_.stPcd.stPcdGroundSeg;
}

CFG_PARAM_PCD_FILTER_STRU &CFG_PARAM_C::GetPcdFilterCfg(void) const
{
    return stCfgParam_.stPcd.stPcdFilter;
}

CFG_PARAM_CONDITION_FILTER_STRU &CFG_PARAM_C::GetPcdConditionFilter(void) const
{
    return stCfgParam_.stPcd.stPcdFilter.stConditionFilter;
}

CFG_PARAM_VOXEL_GRID_FILTER_STRU &CFG_PARAM_C::GetPcdVoexlGridFilter(void) const
{
    return stCfgParam_.stPcd.stPcdFilter.stVoxelGridFilter;
}

CFG_PARAM_RADIUS_OUTLIER_FILTER_STRU &CFG_PARAM_C::GetPcdRadiusOutlierFilter(void) const
{
    return stCfgParam_.stPcd.stPcdFilter.stRadiusOutlier;
}

CFG_PARAM_PCD_PARTITION_STRU &CFG_PARAM_C::GetPcdPartitionCfg(void) const
{
    return stCfgParam_.stPcd.stPcdPartition;
}

CFG_PARAM_PCD_DUSTBIN_FILTER_STRU &CFG_PARAM_C::GetPcdDustbinFilterCfg(void) const
{
    return stCfgParam_.stPcd.stPcdDustbinFilter;
}

CFG_PARAM_DOUBLE_LIDAR_VERIFY_STRU &CFG_PARAM_C::GetDoubleLidarReflectVerifyCfg(void) const
{
    return stCfgParam_.stPcd.stPcdReflectVerify.stDoubleLidarVerify;
}

CFG_PARAM_EMERGENCE_VERIFY_STRU &CFG_PARAM_C::GetEmergenceReflectVerifyCfg(void) const
{
    return stCfgParam_.stPcd.stPcdReflectVerify.stEmergenceVerify;
}

CFG_PARAM_PCD_SLOPE_PROC_STRU &CFG_PARAM_C::GetPcdSlopeProcCfg(void) const
{
    return stCfgParam_.stPcd.stPcdSlopeProc;
}

CFG_PARAM_PCD_FALL_ARREST_STRU &CFG_PARAM_C::GetPcdFallArrestCfg(void) const
{
    return stCfgParam_.stPcd.stPcdFallArrest;
}

CFG_PARAM_PCD_SHORT_OBST_HOLD_STRU &CFG_PARAM_C::GetPcdShortObstHoldCfg(void) const
{
    return stCfgParam_.stPcd.stPcdShortObstHold;
}

CFG_PARAM_PCD_IMPASS_AREA_DET_STRU &CFG_PARAM_C::GetPcdImpassAreaDet(void) const
{
    return stCfgParam_.stPcd.stPcdImpassAreaDet;
}

CFG_PARAM_PCD_CLUSTER_STRU &CFG_PARAM_C::GetPcdClusterCfg(void) const
{
    return stCfgParam_.stPcd.stPcdCluster;
}

CFG_PARAM_INSTALL_STRU &CFG_PARAM_C::GetHeadWideAngleCameraInstallCfg(void) const
{
    return stCfgParam_.stVisionDetect.stHeadWideAngleCam.stInstallPose;
}

CFG_PARAM_INSTALL_STRU &CFG_PARAM_C::GetBackWideAngleCameraInstallCfg(void) const
{
    return stCfgParam_.stVisionDetect.stBackWideAngleCam.stInstallPose;
}

CFG_PARAM_INSTALL_STRU &CFG_PARAM_C::GetHeadLongFocusCameraInstallCfg(void) const
{
    return stCfgParam_.stVisionDetect.stHeadLongFocusCam.stInstallPose;
}

CFG_PARAM_VISION_DETECT_OBJECT_STRU &CFG_PARAM_C::GetHeadWideAngleCamPedCarParam(void) const
{
    return stCfgParam_.stVisionDetect.stHeadWideAngleCam.stDetPedCar;
}

CFG_PARAM_VISION_DETECT_OBJECT_STRU &CFG_PARAM_C::GetBackWideAngleCamPedCarParam(void) const
{
    return stCfgParam_.stVisionDetect.stBackWideAngleCam.stDetPedCar;
}

CFG_PARAM_VISION_DETECT_OBJECT_STRU &CFG_PARAM_C::GetHeadWideAngleCamLaneParam(void) const
{
    return stCfgParam_.stVisionDetect.stHeadWideAngleCam.stDetLane;
}

CFG_PARAM_VISION_DETECT_OBJECT_STRU &CFG_PARAM_C::GetBackWideAngleCamLaneParam(void) const
{
    return stCfgParam_.stVisionDetect.stBackWideAngleCam.stDetLane;
}

CFG_PARAM_VISION_DETECT_OBJECT_STRU &CFG_PARAM_C::GetHeadLongFocusCamTrafficLightParam(void) const
{
    return stCfgParam_.stVisionDetect.stHeadLongFocusCam.stDetTrafficLight;
}

CFG_PARAM_WHEEL_SPEED_SENSOR_STRU &CFG_PARAM_C::GetWheelSpeedSensorCfg(void) const
{
    return stCfgParam_.stVehicleChassis.stWheelSpeedSensor;
}

CFG_PARAM_STEERING_ANGLE_SENSOR_STRU &CFG_PARAM_C::GetSteeringAngleSensorCfg(void) const
{
    return stCfgParam_.stVehicleChassis.stSteeringAngleSensor;
}

CFG_PARAM_VEL_SMOOTHER_STRU &CFG_PARAM_C::GetVelSmootherCfg(void) const
{
    return stCfgParam_.stVehicleChassis.stVelSmoother;
}

const CFG_PARAM_LINEAR_VEL_PLAN_STRU &CFG_PARAM_C::GetLinearVelPLanCfg(void) const
{
    return stCfgParam_.stPlan.stLinearVelPlan;
}

CFG_PARAM_LOC_STRU &CFG_PARAM_C::GetLocCfg(void) const
{
    return stCfgParam_.stLoc;
}

FUSION_METHOD_ENUM &CFG_PARAM_C::GetFusionMethod(void) const
{
    return stCfgParam_.stLoc.stPoseEstimator.enFusionMethod;
}

MATCH_METHOD_ENUM  &CFG_PARAM_C::GetLidarLocMatchMethod(void) const
{
    return stCfgParam_.stLoc.stLidarLoc.enMatchMethod;
}

CFG_PARAM_NDT_MATCHER_STRU &CFG_PARAM_C::GetLidarLocNdtCfg(void) const
{
    return stCfgParam_.stLoc.stLidarLoc.stNdtMatcher;
}

CFG_PARAM_SEARCH_REG_STRU &CFG_PARAM_C::GetReLocSearchRegion(void) const
{
    return stCfgParam_.stLoc.stReloc.stSearchRegion;
}

CFG_PARAM_SEARCH_STEP_STRU &CFG_PARAM_C::GetReLocSearchStep(void) const
{
    return stCfgParam_.stLoc.stReloc.stSearchStep;
}

#endif


